int pinAIN1 = 5; //Direction int pinAIN2 = 4; //Direction int pinPWMA = 9; //Speed //Motor 2 int pinBIN1 = 6; //Direction int pinBIN2 = 7; //Direction int pinPWMB = 10; //Speed //Standby int pinSTBY = 11; //Constants to help remember the parameters static boolean turnCW = 0; //for motorDrive function static boolean turnCCW = 1; //for motorDrive function static boolean motor1 = 0; //for motorDrive, motorStop, motorBrake functions static boolean motor2 = 1; //for motorDrive, motorStop, motorBrake functions // PID-control values float pTerm = 0; float iTerm = 0; float dTerm = 0; float K = 1;//1 float Kp = 50;//25//45=38=38 float Ki = 5;//0//12=18=25 float Kd = 0.4;//7//0.5=0.45 float last_error = 0; float integrated_error = 0; float last_time=0; float time; //Motor value int mSpeed = 0; #define GUARD_GAIN 50//50 #define TARGET_POSITION 0.1 //it was -0.5 /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). Contact information ------------------- Kristian Lauszus, TKJ Electronics Web : http://www.tkjelectronics.com e-mail : kristianl@tkjelectronics.com */ #include #include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter #define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf Kalman kalmanY; Kalman kalmanX; /* IMU Data */ double accX, accY, accZ; double gyroX, gyroY, gyroZ; int16_t tempRaw; double gyroXangle, gyroYangle; // Angle calculate using the gyro only double compAngleX, compAngleY; // Calculated angle using a complementary filter double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter uint32_t timer; uint8_t i2cData[14]; // Buffer for I2C data // TODO: Make calibration routine void setup() { Serial.begin(9600); Wire.begin(); TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz //TCCR1B = TCCR1B & B11111000 | B00000100; // set timer 1 divisor to 256 for PWM frequency of 122.55 Hz //TCCR1B = TCCR1B & B11111000 | B00000101; // set timer 1 divisor to 1024 for PWM frequency of 30.64 Hz TCCR1B = TCCR1B & B11111000 | B00000011; // set timer 1 divisor to 64 for PWM frequency of 490.20 Hz //TCCR1B = TCCR1B & B11111000 | B00000010; // set timer 1 divisor to 8 for PWM frequency of 3921.16 Hz //TCCR1B = TCCR1B & B11111000 | B00000001; // set timer 1 divisor to 1 for PWM frequency of 31372.55 Hz i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode while (i2cRead(0x75, i2cData, 1)); if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register Serial.print(F("Error reading sensor")); while (1); } delay(100); // Wait for sensor to stabilize /* Set kalman and gyro starting angle */ while (i2cRead(0x3B, i2cData, 6)); accX = (i2cData[0] << 8) | i2cData[1]; accY = (i2cData[2] << 8) | i2cData[3]; accZ = (i2cData[4] << 8) | i2cData[5]; // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26 // atan2 outputs the value of -p to p (radians) - see http://en.wikipedia.org/wiki/Atan2 // It is then converted from radians to degrees #ifdef RESTRICT_PITCH // Eq. 25 and 26 double roll = atan2(accY, accZ) * RAD_TO_DEG; double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; #else // Eq. 28 and 29 double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; double pitch = atan2(-accX, accZ) * RAD_TO_DEG; #endif kalmanX.setAngle(roll); // Set starting angle kalmanY.setAngle(pitch); gyroXangle = roll; gyroYangle = pitch; compAngleX = roll; compAngleY = pitch; timer = micros(); } void loop() { /* Update all the values */ while (i2cRead(0x3B, i2cData, 14)); accX = ((i2cData[0] << 8) | i2cData[1]); accY = ((i2cData[2] << 8) | i2cData[3]); accZ = ((i2cData[4] << 8) | i2cData[5]); tempRaw = (i2cData[6] << 8) | i2cData[7]; gyroX = (i2cData[8] << 8) | i2cData[9]; gyroY = (i2cData[10] << 8) | i2cData[11]; gyroZ = (i2cData[12] << 8) | i2cData[13]; double dt = (double)(micros() - timer) / 1000000; // Calculate delta time timer = micros(); // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26 // atan2 outputs the value of -p to p (radians) - see http://en.wikipedia.org/wiki/Atan2 // It is then converted from radians to degrees #ifdef RESTRICT_PITCH // Eq. 25 and 26 double roll = atan2(accY, accZ) * RAD_TO_DEG; double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; #else // Eq. 28 and 29 double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; double pitch = atan2(-accX, accZ) * RAD_TO_DEG; #endif double gyroXrate = gyroX / 131.0; // Convert to deg/s double gyroYrate = gyroY / 131.0; // Convert to deg/s #ifdef RESTRICT_PITCH // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) { kalmanX.setAngle(roll); compAngleX = roll; kalAngleX = roll; gyroXangle = roll; } else kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter if (abs(kalAngleX) > 90) gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); #else // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) { kalmanY.setAngle(pitch); compAngleY = pitch; kalAngleY = pitch; gyroYangle = pitch; } else kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter if (abs(kalAngleY) > 90) gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter #endif gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter gyroYangle += gyroYrate * dt; //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate //gyroYangle += kalmanY.getRate() * dt; compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch; // Reset the gyro angle when it has drifted too much if (gyroXangle < -180 || gyroXangle > 180) gyroXangle = kalAngleX; if (gyroYangle < -180 || gyroYangle > 180) gyroYangle = kalAngleY; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////MY ADDIDATION CODE ///////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////PID CONTROLE//////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //K=exp(abs(kalAngleY))/10000; mSpeed = updatePid(TARGET_POSITION, kalAngleY,dt); /*if (abs(mSpeed)>15 & abs(mSpeed)<55){ if(mSpeed<-10 & mSpeed>-20){mSpeed=mSpeed-40;} if(mSpeed<-20 & mSpeed>-30){mSpeed=mSpeed-30;} if(mSpeed<-30 & mSpeed>-40){mSpeed=mSpeed-20;} if(mSpeed<-40 & mSpeed>-50){mSpeed=mSpeed-10;} if(mSpeed<-50 & mSpeed>-55){mSpeed=mSpeed-5;} // if(mSpeed<-15 & mSpeed>-55){mSpeed=mSpeed-20;} if(mSpeed<20 & mSpeed>10){mSpeed=mSpeed+40;} if(mSpeed<30 & mSpeed>20){mSpeed=mSpeed+30;} if(mSpeed<40 & mSpeed>30){mSpeed=mSpeed+20;} if(mSpeed<50 & mSpeed>40){mSpeed=mSpeed+10;} if(mSpeed<55 & mSpeed>50){mSpeed=mSpeed+5;} }*/ //mSpeed = map(mSpeed, -255, 255, 0, 255); if (mSpeed>=0 & abs(kalAngleY)<30) { motorDrive(motor1, turnCW, mSpeed); //go back motorDrive(motor2, turnCW, mSpeed); } if (mSpeed<0 & abs(kalAngleY)<30) { motorDrive(motor1, turnCCW, abs(mSpeed)); //go back motorDrive(motor2, turnCCW, abs(mSpeed)); } if(abs(kalAngleY)>=30){ motorBrake(motor1); motorBrake(motor2); motorStop(motor1); motorStop(motor2); } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* Print Data */ #if 0 // Set to 1 to activate Serial.print(accX); Serial.print("\t"); Serial.print(accY); Serial.print("\t"); Serial.print(accZ); Serial.print("\t"); Serial.print(gyroX); Serial.print("\t"); Serial.print(gyroY); Serial.print("\t"); Serial.print(gyroZ); Serial.print("\t"); Serial.print("\t"); #endif /* Serial.print(roll); Serial.print("\t"); Serial.print(gyroXangle); Serial.print("\t"); Serial.print(compAngleX); Serial.print("\t"); Serial.print(kalAngleX); Serial.print("\t"); Serial.print("\t");*/ Serial.print(pitch); Serial.print("\t"); Serial.print(gyroYangle); Serial.print("\t"); Serial.print(compAngleY); Serial.print("\t"); Serial.print(kalAngleY); Serial.print("\t"); Serial.print(mSpeed); Serial.print("\t"); Serial.print(pTerm); Serial.print("\t"); Serial.print(iTerm); Serial.print("\t"); Serial.print(dTerm); Serial.print("\t"); #if 0 // Set to 1 to print the temperature Serial.print("\t"); double temperature = (double)tempRaw / 340.0 + 36.53; Serial.print(temperature); Serial.print("\t"); #endif Serial.print("\r\n"); delay(2); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////// FUNCTION ////////////////////////////////////////////////////////// // ================================================================ // === PID-CONTROL === // ================================================================ int updatePid(float targetPosition, float currentPosition, float dt) { float error = targetPosition - currentPosition; pTerm = Kp * error; integrated_error+=error*dt; iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN); dTerm = Kd * (error - last_error)/dt; last_error = error; return -constrain(K*(pTerm + iTerm + dTerm), -255, 255); // (...dTerm), -255, 255); /* Serial.pr>CXVint("\t pTerm:");// Serial.print(K*(pTerm));// Serial.print("\t iTerm: ");// Serial.print(K*iTerm);// Serial.print("\t dTerm:");// Serial.print(K*dTerm); Serial.print("\t PID:"); Serial.print(-constrain(K*(pTerm + iTerm + dTerm), -255, 255)); */ } void motorDrive(boolean motorNumber, boolean motorDirection, int motorSpeed) { /* This Drives a specified motor, in a specific direction, at a specified speed: - motorNumber: motor1 or motor2 ---> Motor 1 or Motor 2 - motorDirection: turnCW or turnCCW ---> clockwise or counter-clockwise - motorSpeed: 0 to 255 ---> 0 = stop / 255 = fast */ boolean pinIn1; //Relates to AIN1 or BIN1 (depending on the motor number specified) //Specify the Direction to turn the motor //Clockwise: AIN1/BIN1 = HIGH and AIN2/BIN2 = LOW //Counter-Clockwise: AIN1/BIN1 = LOW and AIN2/BIN2 = HIGH if (motorDirection == turnCW) pinIn1 = HIGH; else pinIn1 = LOW; //Select the motor to turn, and set the direction and the speed if(motorNumber == motor1) { digitalWrite(pinAIN1, pinIn1); digitalWrite(pinAIN2, !pinIn1); //This is the opposite of the AIN1 analogWrite(pinPWMA, motorSpeed); } else { digitalWrite(pinBIN1, pinIn1); digitalWrite(pinBIN2, !pinIn1); //This is the opposite of the BIN1 analogWrite(pinPWMB, motorSpeed); } //Finally , make sure STBY is disabled - pull it HIGH digitalWrite(pinSTBY, HIGH); } void motorBrake(boolean motorNumber) { /* This "Short Brake"s the specified motor, by setting speed to zero */ if (motorNumber == motor1) analogWrite(pinPWMA, 0); else analogWrite(pinPWMB, 0); } void motorStop(boolean motorNumber) { /* This stops the specified motor by setting both IN pins to LOW */ if (motorNumber == motor1) { digitalWrite(pinAIN1, LOW); digitalWrite(pinAIN2, LOW); } else { digitalWrite(pinBIN1, LOW); digitalWrite(pinBIN2, LOW); } } void motorsStandby() { /* This puts the motors into Standby Mode */ digitalWrite(pinSTBY, LOW); }