diff --git a/Main/IMU.ino b/Main/IMU.ino index 27a593a..155b71a 100644 --- a/Main/IMU.ino +++ b/Main/IMU.ino @@ -1,5 +1,4 @@ //IMU variables - int ax_, ay_, az_; int ax, ay, az; int cx, cy, cz; @@ -7,18 +6,18 @@ float gx, gy, gz, gt; float acc_pitch; void readIMU() { - ax_ = GY85.accelerometer_x( GY85.readFromAccelerometer() ); - ay_ = GY85.accelerometer_y( GY85.readFromAccelerometer() ); - az_ = GY85.accelerometer_z( GY85.readFromAccelerometer() ); + ax_ = IMU.accelerometer_x( IMU.readFromAccelerometer() ); + ay_ = IMU.accelerometer_y( IMU.readFromAccelerometer() ); + az_ = IMU.accelerometer_z( IMU.readFromAccelerometer() ); - // cx = GY85.compass_x( GY85.readFromCompass() ); - // cy = GY85.compass_y( GY85.readFromCompass() ); - // cz = GY85.compass_z( GY85.readFromCompass() ); + // cx = IMU.compass_x( IMU.readFromCompass() ); + // cy = IMU.compass_y( IMU.readFromCompass() ); + // cz = IMU.compass_z( IMU.readFromCompass() ); - gx = GY85.gyro_x( GY85.readGyro() ); - gy = GY85.gyro_y( GY85.readGyro() ); - gz = GY85.gyro_z( GY85.readGyro() ); - // gt = GY85.temp ( GY85.readGyro() ); + gx = IMU.gyro_x( IMU.readGyro() ); + gy = IMU.gyro_y( IMU.readGyro() ); + gz = IMU.gyro_z( IMU.readGyro() ); + // gt = IMU.temp ( IMU.readGyro() ); //For some reason the ints in the example behaves as unsigned int.. Maybe look at the .cpp code, might be something there, if not. This works OK. diff --git a/Main/Main.ino b/Main/Main.ino index 06d2cc8..e83c16f 100644 --- a/Main/Main.ino +++ b/Main/Main.ino @@ -2,78 +2,148 @@ #include "GY_85.h" #include -GY_85 GY85; //create the object + +//Declare library objects +GY_85 IMU; + + +//GPIO PIN MAPPING +const byte M1_ENC_A = 34; +const byte M1_ENC_B = 35; +const byte M2_ENC_A = 32; +const byte M2_ENC_B = 33; +const byte M1_A = 18; +const byte M1_B = 19; +const byte M2_A = 16; +const byte M2_B = 17; +const byte IMU_I2C_SCL = 26; +const byte IMU_I2C_SDA = 27; + //System variables -unsigned long tNow = micros(); +unsigned long tNow = micros(); -//Motor Control variables -const byte m1_in1 = 18; -const byte m1_in2 = 19; -//int m2_in1 = 16; -//int m2_in2 = 17; + +//Motor variables +const int PWM_CYCLE = 12000; +const byte PWM_RESOLUTION = 12; //Encoders variables -long int m1Raw,m1RawLast; -const byte pin_m1_A = 34; -const byte pin_m1_B = 35; -volatile bool A_state; -volatile bool B_state; +long int m1Raw, m1RawLast; +long int m2Raw, m2RawLast; +volatile bool M1_A_state; +volatile bool M1_B_state; +volatile bool M2_A_state; +volatile bool M2_B_state; + //Interrupt routines // CW = INCREASE // CCW = DECREASE + void IRAM_ATTR m1_A_changed() { - A_state = digitalRead(pin_m1_A); - B_state = digitalRead(pin_m1_B); - + M1_A_state = digitalRead(M1_ENC_A); + M1_B_state = digitalRead(M1_ENC_B); + //Rising - if (A_state == HIGH) { - if (B_state == HIGH) { + if (M1_A_state == HIGH) { + if (M1_B_state == HIGH) { m1Raw = m1Raw - 1; } - else if (B_state == LOW) { + else if (M1_B_state == LOW) { m1Raw = m1Raw + 1; } } - + //Falling - else if (A_state == LOW) { - if (B_state == HIGH) { + else if (M1_A_state == LOW) { + if (M1_B_state == HIGH) { m1Raw = m1Raw + 1; } - else if (B_state == LOW) { + else if (M1_B_state == LOW) { m1Raw = m1Raw - 1; } } } + void IRAM_ATTR m1_B_changed() { - A_state = digitalRead(pin_m1_A); - B_state = digitalRead(pin_m1_B); - + M1_A_state = digitalRead(M1_ENC_A); + M1_B_state = digitalRead(M1_ENC_B); + //Rising - if (B_state == HIGH) { - if (A_state == HIGH) { + if (M1_B_state == HIGH) { + if (M1_A_state == HIGH) { m1Raw = m1Raw + 1; } - else if (A_state == LOW) { + else if (M1_A_state == LOW) { m1Raw = m1Raw - 1; } } //Falling - else if (B_state == LOW) { - if (A_state == HIGH) { + else if (M1_B_state == LOW) { + if (M1_A_state == HIGH) { m1Raw = m1Raw - 1; } - else if (A_state == LOW) { + else if (M1_A_state == LOW) { m1Raw = m1Raw + 1; } } } +void IRAM_ATTR m2_A_changed() { + M2_A_state = digitalRead(M2_ENC_A); + M2_B_state = digitalRead(M2_ENC_B); + + //Rising + if (M2_A_state == HIGH) { + if (M2_B_state == HIGH) { + m2Raw = m2Raw - 1; + } + else if (M2_B_state == LOW) { + m2Raw = m2Raw + 1; + } + } + + //Falling + else if (M2_A_state == LOW) { + if (M2_B_state == HIGH) { + m2Raw = m2Raw + 1; + } + else if (M2_B_state == LOW) { + m2Raw = m2Raw - 1; + } + } +} + + +void IRAM_ATTR m2_B_changed() { + M2_A_state = digitalRead(M2_ENC_A); + M2_B_state = digitalRead(M2_ENC_B); + + //Rising + if (M2_B_state == HIGH) { + if (M2_A_state == HIGH) { + m2Raw = m2Raw + 1; + } + else if (M2_A_state == LOW) { + m2Raw = m2Raw - 1; + } + } + + //Falling + else if (M2_B_state == LOW) { + if (M2_A_state == HIGH) { + m2Raw = m2Raw - 1; + } + else if (M2_A_state == LOW) { + m2Raw = m2Raw + 1; + } + } +} + void setup() { //Initialize serial @@ -81,39 +151,44 @@ void setup() { delay(10); //Initialice I2C - // Wire.begin(SCL,SDA); - Wire.begin(26, 27); + Wire.begin(IMU_I2C_SCL, IMU_I2C_SDA); delay(10); //Initialize IMU - GY85.init(); + IMU.init(); delay(10); - //Initialize Interrupts - pinMode(pin_m1_A, INPUT_PULLUP); - pinMode(pin_m1_B, INPUT_PULLUP); - attachInterrupt(digitalPinToInterrupt(pin_m1_A), m1_A_changed, CHANGE); - attachInterrupt(digitalPinToInterrupt(pin_m1_B), m1_B_changed, CHANGE); + //Initialize encoder interrupts + pinMode(M1_ENC_A, INPUT_PULLUP); + pinMode(M1_ENC_B, INPUT_PULLUP); + pinMode(M2_ENC_A, INPUT_PULLUP); + pinMode(M2_ENC_B, INPUT_PULLUP); + attachInterrupt(digitalPinToInterrupt(M1_ENC_A), m1_A_changed, CHANGE); + attachInterrupt(digitalPinToInterrupt(M1_ENC_B), m1_B_changed, CHANGE); + attachInterrupt(digitalPinToInterrupt(M2_ENC_A), m2_A_changed, CHANGE); + attachInterrupt(digitalPinToInterrupt(M2_ENC_B), m2_B_changed, CHANGE); - //Initialize Encoders + //Initialize encoders m1Raw = 0; m1RawLast = 100; + m2Raw = 0; + m2RawLast = 100; // Initialize PWM channels // channels 0-15, resolution 1-16 bits, freq limits depend on resolution // ledcSetup(uint8_t channel, uint32_t freq, uint8_t resolution_bits); - ledcAttachPin(m1_in1, 1); - ledcAttachPin(m1_in2, 2); - // ledcAttachPin(m2_in1, 3); - // ledcAttachPin(m2_in2, 4); + ledcAttachPin(M1_A, 1); + ledcAttachPin(M1_B, 2); + ledcAttachPin(M2_A, 3); + ledcAttachPin(M2_B, 4); - ledcSetup(1, 12000, 12); // 12 kHz PWM, 8-bit resolution - ledcSetup(2, 12000, 12); - // ledcSetup(3, 12000, 8); - // ledcSetup(4, 12000, 8); + ledcSetup(1, PWM_CYCLE, PWM_RESOLUTION); + ledcSetup(2, PWM_CYCLE, PWM_RESOLUTION); + ledcSetup(3, PWM_CYCLE, PWM_RESOLUTION); + ledcSetup(4, PWM_CYCLE, PWM_RESOLUTION); -Serial.println("Reference,Actual,SpeedCommand"); + Serial.println("Reference,Actual,SpeedCommand"); } @@ -121,11 +196,11 @@ void loop() { //Update system variables tNow = micros(); -//// //Only print encoder value if value changed since last print -// if (m1Raw != m1RawLast) { -// Serial.println(m1Raw); -// m1RawLast = m1Raw; -// } + //// //Only print encoder value if value changed since last print + // if (m1Raw != m1RawLast) { + // Serial.println(m1Raw); + // m1RawLast = m1Raw; + // } //Sense readIMU(); diff --git a/Main/motorControl.ino b/Main/motorControl.ino index 89189ed..dd42583 100644 --- a/Main/motorControl.ino +++ b/Main/motorControl.ino @@ -1,40 +1,43 @@ //Constants -const float wheel_diameter = 0.067708; -const float pulses_per_turn = 1320.0; +const float wheel_diameter = 0.067708; +const float pulses_per_turn = 1320.0; //Tuning -const float K = 2; -const float I = 10; +const float K = 2.0; +const float I = 10.0; //Help variables -int dEnc, dT; -float dTurn, ang_vel, lin_vel, ang_vel_filtered, lin_vel_filtered; +int dEnc, dT; +float dTurn, ang_vel, lin_vel, ang_vel_filtered, lin_vel_filtered; unsigned long tLast; -float speed_setp,referenceSpeed,actualSpeed; -float error, iError; -int speedCMD; +float speed_setp, referenceSpeed, actualSpeed; +float error, iError; +int speedCMD; void motorControl() { - //Motor 1 - speed_setp = map(acc_pitch, -90.0, 90.0, -4096, 4096); - referenceSpeed = speed_setp; - actualSpeed = lin_vel*4096; - error = referenceSpeed - actualSpeed; - iError = iError + (error*dT*pow(10,-6)*I); - speedCMD = round((error*K)+iError); -// Saturation - if(speedCMD>4096){ + //Speed Controller + referenceSpeed = acc_pitch * ((4096.0) / (90.0)); + actualSpeed = lin_vel * 4096.0; + error = referenceSpeed - actualSpeed; + iError = iError + (error * dT * pow(10, -6) * I); + speedCMD = round((error * K) + iError); + + + // Speed command saturation + if (speedCMD > 4096) { speedCMD = 4096; } - else if(speedCMD < -4096){ + else if (speedCMD < -4096) { speedCMD = -4096; } - else{ + else { speedCMD = speedCMD; } - + + + //Motor 1 Control if (speedCMD > 0) { ledcWrite(1, 0); ledcWrite(2, speedCMD); @@ -56,27 +59,27 @@ void motorControl() { // ledcWrite(3, 255); // ledcWrite(4, 255); - //Calculate speed from encoders - dEnc = m1Raw - m1RawLast; //[pulses] - dT = tNow - tLast; //[micro sec] - dTurn = dEnc / pulses_per_turn; - ang_vel = (dTurn * 2 * PI) /(dT*0.000001); - lin_vel = (dTurn * wheel_diameter * PI) /(dT*0.000001); + //Calculate speed from encoders + dEnc = m1Raw - m1RawLast; //[Number of encoder pulses this cycle] + dT = tNow - tLast; //[Cycle time in microseconds] + dTurn = dEnc / pulses_per_turn; //[Amount wheel turned this cycle. 1 = full rotation] + + ang_vel = (dTurn * 2 * PI) / (dT * 0.000001); + lin_vel = (dTurn * wheel_diameter * PI) / (dT * 0.000001); Serial.print(referenceSpeed); Serial.print ( "," ); - Serial.println(actualSpeed); -// Serial.print ( "," ); -// Serial.println(speedCMD); -// Serial.print ( "," ); -// Serial.println(lin_vel); -// Serial.print ( "," ); + Serial.print(actualSpeed); + Serial.print ( "," ); + Serial.println(speedCMD); + // Serial.print ( "," ); + // Serial.println(lin_vel); + // Serial.print ( "," ); - //update variables for next scan cycle + + //Update variables for next scan cycle m1RawLast = m1Raw; tLast = tNow; - - }