diff --git a/Main/IMU.ino b/Main/IMU.ino index 2a7d5a5..d62f40d 100644 --- a/Main/IMU.ino +++ b/Main/IMU.ino @@ -30,15 +30,9 @@ void readIMU() { // Gyrocope - // Coordinate system - // gx - Pitch rate - // gy - Roll rate - // gz - Yaw rate - // Gyro is calibrated for +-2000deg/s - // Conversion is happening in GY_85.h line 48-50 - gx = convertInt(IMU.gyro_x( IMU.readGyro() ), gyro_overflow_value); - gy = convertInt(IMU.gyro_y( IMU.readGyro() ), gyro_overflow_value); - gz = convertInt(IMU.gyro_z( IMU.readGyro() ), gyro_overflow_value); + gx = convertInt(IMU.gyro_x( IMU.readGyro() ), gyro_overflow_value); // gx - Pitch rate + gy = convertInt(IMU.gyro_y( IMU.readGyro() ), gyro_overflow_value); // gy - Roll rate + gz = convertInt(IMU.gyro_z( IMU.readGyro() ), gyro_overflow_value); // gz - Yaw rate //Temperature sensor @@ -58,20 +52,7 @@ void readIMU() { pitch = acc_pitch * (1 - alpha) + ((dAngle + pitch_prev) * alpha); pitch_prev = pitch; - - //Serial plotter - // Serial.print ( "Pitch:" ); - // Serial.print ( pitch ); - // Serial.print (" "); - // Serial.print ( "Accelerometer_Pitch:" ); - // Serial.print ( acc_pitch ); - // Serial.print (" "); - // Serial.print ( "," ); - // Serial.println ( gz ); - // Serial.print ( "," ); - // Serial.println ( gt ); - // Serial.print ( "," ); - // Serial.println ( acc_pitch); + } diff --git a/Main/Main.ino b/Main/Main.ino index b63a292..5cc159b 100644 --- a/Main/Main.ino +++ b/Main/Main.ino @@ -5,7 +5,7 @@ //Declare library objects -GY_85 IMU; +GY_85 IMU; //GPIO PIN MAPPING @@ -41,16 +41,12 @@ volatile bool M2_A_state, M2_B_state; //Matrices -mtx_type motor_ang_vel [2][1]; -mtx_type vel_Matrix [2][1]; -mtx_type inv_Kin [2][2]; +mtx_type motor_ang_vel [2][1]; +mtx_type vel_Matrix [2][1]; +mtx_type inv_Kin [2][2]; //Interrupt routines -// CW = INCREASE -// CCW = DECREASE - - void IRAM_ATTR m1_A_changed() { M1_A_state = digitalRead(M1_ENC_A); M1_B_state = digitalRead(M1_ENC_B); @@ -210,24 +206,21 @@ void loop() { dT = tNow - tLast; //[Cycle time in microseconds] dT_s = dT * pow(10,-6); //[Cycle time in seconds] -// Serial.print("dT:"); -// Serial.print(dT); -// Serial.print(" "); -// Serial.print("dT_s:"); -// Serial.println(dT_s); - //Get sensor data readIMU(); - //Control motor + //Control motors motors(); - //Save time + //Save time for next cycle tLast = tNow; + //Plot + // plot(); + //Delay delay(5); // only read every 0,5 seconds, 10ms for 100Hz, 20ms for 50Hz diff --git a/Main/motorControl.ino b/Main/motorControl.ino index cd84c0c..94bef12 100644 --- a/Main/motorControl.ino +++ b/Main/motorControl.ino @@ -5,6 +5,7 @@ const float WHEEL_DIAMETER = 0.0677; const float PULSES_PER_TURN = 1320.0; const float BALANCE_POINT = 0.05; const float SPEED_REF = 0.00; +const float TURN_SPEED_REF = 0.00; const float DEADBAND_M1_POS = 90.0; const float DEADBAND_M1_NEG = 90.0; const float DEADBAND_M2_POS = 90.0; @@ -85,39 +86,10 @@ void motors() { motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG); - // Serial plotter - // Serial.print("Balance_Point:"); - // Serial.print(ref_OL); - // Serial.print(" "); - // Serial.print("Pitch_Angle:"); - // Serial.print(act_OL); - // Serial.print(" "); - // Serial.print("Speed_CMD:"); - // Serial.println(Speed_CMD * (100.0 / 4096.0)); - - - // Serial.print("M1_Ang_Vel:"); - // Serial.print(M1_Ang_Vel); - // Serial.print(" "); - // Serial.print("M2_Ang_Vel:"); - // Serial.print(M2_Ang_Vel); - // Serial.print(" "); - // Serial.print("botLinVel:"); - // Serial.print(vel_Matrix[0][0]); - // Serial.print(" "); - // Serial.print("botAngVel:"); - // Serial.println(vel_Matrix[1][0]); - - //Update variables for next scan cycle m1RawLast = m1Raw; m2RawLast = m2Raw; - // Serial.print("m1Raw:"); - // Serial.print(m1Raw); - // Serial.print(" "); - // Serial.print("m2Raw:"); - // Serial.println(m2Raw); } @@ -137,9 +109,8 @@ float encoderReaderAngVel(int encRaw, int encRawLast, float ang_vel_filtered_, f void motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, float dbNeg_) { //Calculate channel - byte ch1 = motorID * 2 - 1; byte ch2 = motorID * 2; - + byte ch1 = ch2 - 1; //Deadband if (speedCMD_ > 0 && speedCMD_ < dbPos_) { diff --git a/Main/plotter.ino b/Main/plotter.ino new file mode 100644 index 0000000..3c76bd8 --- /dev/null +++ b/Main/plotter.ino @@ -0,0 +1,54 @@ +void plot(){ + +// Time + // Serial.print("dT:"); + // Serial.print(dT); + // Serial.print(" "); + // Serial.print("dT_s:"); + // Serial.println(dT_s); + +// IMU + // Serial.print ( "Pitch:" ); + // Serial.print ( pitch ); + // Serial.print (" "); + // Serial.print ( "Accelerometer_Pitch:" ); + // Serial.print ( acc_pitch ); + // Serial.print (" "); + // Serial.print ( "," ); + // Serial.println ( gz ); + // Serial.print ( "," ); + // Serial.println ( gt ); + // Serial.print ( "," ); + // Serial.println ( acc_pitch); + + +// Motors + // Serial plotter + // Serial.print("Balance_Point:"); + // Serial.print(ref_OL); + // Serial.print(" "); + // Serial.print("Pitch_Angle:"); + // Serial.print(act_OL); + // Serial.print(" "); + // Serial.print("Speed_CMD:"); + // Serial.println(Speed_CMD * (100.0 / 4096.0)); + + // Serial.print("M1_Ang_Vel:"); + // Serial.print(M1_Ang_Vel); + // Serial.print(" "); + // Serial.print("M2_Ang_Vel:"); + // Serial.print(M2_Ang_Vel); + // Serial.print(" "); + // Serial.print("botLinVel:"); + // Serial.print(vel_Matrix[0][0]); + // Serial.print(" "); + // Serial.print("botAngVel:"); + // Serial.println(vel_Matrix[1][0]); + +// Encoders + // Serial.print("m1Raw:"); + // Serial.print(m1Raw); + // Serial.print(" "); + // Serial.print("m2Raw:"); + // Serial.println(m2Raw); +}