Berk Yaşar YAVUZ / Mbed 2 deprecated balancebot

balance

Dependencies:   mbed LSM9DS0

main.cpp@0:9d2ceb7b8ec9, 2015-10-20 (annotated)

Committer:
omidece2035
Date:
Tue Oct 20 04:32:01 2015 +0000
Revision:
0:9d2ceb7b8ec9
V1 for ECE  4180 LAB4

Who changed what in which revision?

UserRevisionLine numberNew contents of line
omidece2035 0:9d2ceb7b8ec9 1 #include "mbed.h"
omidece2035 0:9d2ceb7b8ec9 2 #include "LSM9DS0.h"
omidece2035 0:9d2ceb7b8ec9 3 #include "math.h"
omidece2035 0:9d2ceb7b8ec9 4
omidece2035 0:9d2ceb7b8ec9 5 LSM9DS0 imu(p9, p10, 0x6B, 0x1D);
omidece2035 0:9d2ceb7b8ec9 6 Serial pc(USBTX, USBRX);
omidece2035 0:9d2ceb7b8ec9 7 Ticker control; // create Ticker for control algorithm
omidece2035 0:9d2ceb7b8ec9 8 Ticker db;
omidece2035 0:9d2ceb7b8ec9 9
omidece2035 0:9d2ceb7b8ec9 10 //debugging
omidece2035 0:9d2ceb7b8ec9 11 int debug=0;
omidece2035 0:9d2ceb7b8ec9 12 int wt = 500;
omidece2035 0:9d2ceb7b8ec9 13 DigitalOut led1(LED1);
omidece2035 0:9d2ceb7b8ec9 14
omidece2035 0:9d2ceb7b8ec9 15 //motor variables
omidece2035 0:9d2ceb7b8ec9 16 PwmOut left_motor_speed(p21); //left motor pwm A
omidece2035 0:9d2ceb7b8ec9 17 PwmOut right_motor_speed(p22); //right motor pwm B
omidece2035 0:9d2ceb7b8ec9 18 DigitalOut left_fwd_enable(p23); //fwd enable of left/A motor
omidece2035 0:9d2ceb7b8ec9 19 DigitalOut left_rev_enable(p24); //rev enable of left/A motor
omidece2035 0:9d2ceb7b8ec9 20 DigitalOut right_fwd_enable(p26); //fwd enable or right/B motor
omidece2035 0:9d2ceb7b8ec9 21 DigitalOut right_rev_enable(p25); //rev enable of right/B motor
omidece2035 0:9d2ceb7b8ec9 22
omidece2035 0:9d2ceb7b8ec9 23 //angle variables
omidece2035 0:9d2ceb7b8ec9 24 float accelerometer_angle =0;
omidece2035 0:9d2ceb7b8ec9 25 float gyro_data =0;
omidece2035 0:9d2ceb7b8ec9 26 float gyro_angle=0;
omidece2035 0:9d2ceb7b8ec9 27 float current_angle =0;
omidece2035 0:9d2ceb7b8ec9 28 float angle_offset = 0;
omidece2035 0:9d2ceb7b8ec9 29 int i=0;
omidece2035 0:9d2ceb7b8ec9 30
omidece2035 0:9d2ceb7b8ec9 31 //control variables
omidece2035 0:9d2ceb7b8ec9 32 float Error=0; // I had to capitalize this for some reason unlike the other vars
omidece2035 0:9d2ceb7b8ec9 33 float kp=.5;
omidece2035 0:9d2ceb7b8ec9 34 float ki=.01;
omidece2035 0:9d2ceb7b8ec9 35 float kd= 0;
omidece2035 0:9d2ceb7b8ec9 36 float setpoint=0; //reference angle
omidece2035 0:9d2ceb7b8ec9 37 float angle=0;
omidece2035 0:9d2ceb7b8ec9 38 float integral=0;
omidece2035 0:9d2ceb7b8ec9 39 float derivative = 0;
omidece2035 0:9d2ceb7b8ec9 40 float dt= 0.01; //time period 1/100 (s)
omidece2035 0:9d2ceb7b8ec9 41 float previous_error =0;
omidece2035 0:9d2ceb7b8ec9 42 float actuator_input =0;
omidece2035 0:9d2ceb7b8ec9 43 int ready = 0;
omidece2035 0:9d2ceb7b8ec9 44 float maximum_angle=15;
omidece2035 0:9d2ceb7b8ec9 45 float SCALAR = 0;
omidece2035 0:9d2ceb7b8ec9 46
omidece2035 0:9d2ceb7b8ec9 47 void initialiation (void)
omidece2035 0:9d2ceb7b8ec9 48 {
omidece2035 0:9d2ceb7b8ec9 49 //with the "initialization" in the control loop, we may not use this
omidece2035 0:9d2ceb7b8ec9 50 //this is where offsets and sensors are set up
omidece2035 0:9d2ceb7b8ec9 51 //need to hold robot balanced at this point
omidece2035 0:9d2ceb7b8ec9 52 imu.readGyro();
omidece2035 0:9d2ceb7b8ec9 53 angle_offset= float(imu.gy); //maybe take avg. like the other team did in a for loop
omidece2035 0:9d2ceb7b8ec9 54 }
omidece2035 0:9d2ceb7b8ec9 55
omidece2035 0:9d2ceb7b8ec9 56 void MotorDriver (float speed)
omidece2035 0:9d2ceb7b8ec9 57 {
omidece2035 0:9d2ceb7b8ec9 58 // if speed > 0 then move robot left
omidece2035 0:9d2ceb7b8ec9 59 // if speed < 0 then move robot right
omidece2035 0:9d2ceb7b8ec9 60 if ( speed>0 )
omidece2035 0:9d2ceb7b8ec9 61 {
omidece2035 0:9d2ceb7b8ec9 62 //move both motors in same direction
omidece2035 0:9d2ceb7b8ec9 63 right_rev_enable = 0;
omidece2035 0:9d2ceb7b8ec9 64 right_fwd_enable = 1;
omidece2035 0:9d2ceb7b8ec9 65 left_fwd_enable = 0;
omidece2035 0:9d2ceb7b8ec9 66 left_rev_enable = 1;
omidece2035 0:9d2ceb7b8ec9 67 right_motor_speed = speed;
omidece2035 0:9d2ceb7b8ec9 68 left_motor_speed = speed;
omidece2035 0:9d2ceb7b8ec9 69 //pc.printf("moving f\n");
omidece2035 0:9d2ceb7b8ec9 70 }
omidece2035 0:9d2ceb7b8ec9 71 else if ( speed <0 )
omidece2035 0:9d2ceb7b8ec9 72 {
omidece2035 0:9d2ceb7b8ec9 73 //move both motors in same direction
omidece2035 0:9d2ceb7b8ec9 74 right_fwd_enable = 0;
omidece2035 0:9d2ceb7b8ec9 75 right_rev_enable = 1;
omidece2035 0:9d2ceb7b8ec9 76 left_rev_enable = 0;
omidece2035 0:9d2ceb7b8ec9 77 left_fwd_enable = 1;
omidece2035 0:9d2ceb7b8ec9 78 right_motor_speed = -speed;
omidece2035 0:9d2ceb7b8ec9 79 left_motor_speed= -speed;
omidece2035 0:9d2ceb7b8ec9 80 //pc.printf("moving r\n");
omidece2035 0:9d2ceb7b8ec9 81 }
omidece2035 0:9d2ceb7b8ec9 82 else
omidece2035 0:9d2ceb7b8ec9 83 {
omidece2035 0:9d2ceb7b8ec9 84 right_fwd_enable = 0;
omidece2035 0:9d2ceb7b8ec9 85 right_rev_enable = 0;
omidece2035 0:9d2ceb7b8ec9 86 left_rev_enable = 0;
omidece2035 0:9d2ceb7b8ec9 87 left_fwd_enable = 0;
omidece2035 0:9d2ceb7b8ec9 88 right_motor_speed = speed;
omidece2035 0:9d2ceb7b8ec9 89 left_motor_speed = speed;
omidece2035 0:9d2ceb7b8ec9 90 //pc.printf("not moving\n");
omidece2035 0:9d2ceb7b8ec9 91 }
omidece2035 0:9d2ceb7b8ec9 92 }
omidece2035 0:9d2ceb7b8ec9 93
omidece2035 0:9d2ceb7b8ec9 94 float calcSCALAR(float degrees)
omidece2035 0:9d2ceb7b8ec9 95 {
omidece2035 0:9d2ceb7b8ec9 96 //give it a degree such as 10 degrees, anything past that is the same as 10 degrees
omidece2035 0:9d2ceb7b8ec9 97 Error = 0 - degrees; //this is the maximum error
omidece2035 0:9d2ceb7b8ec9 98 integral = integral + (Error*dt); //Forward Euler approximation
omidece2035 0:9d2ceb7b8ec9 99 derivative = (Error - previous_error) / dt; //change wrt time
omidece2035 0:9d2ceb7b8ec9 100 previous_error = Error;
omidece2035 0:9d2ceb7b8ec9 101 SCALAR = ( (Error*kp) + (integral*ki) + (derivative*kd) );
omidece2035 0:9d2ceb7b8ec9 102 pc.printf("here in scalar calc and error is %f and integral is %f and derivative is %f \n", Error, integral, derivative);
omidece2035 0:9d2ceb7b8ec9 103 Error=0;
omidece2035 0:9d2ceb7b8ec9 104 integral=0;
omidece2035 0:9d2ceb7b8ec9 105 derivative=0;
omidece2035 0:9d2ceb7b8ec9 106 previous_error=0;
omidece2035 0:9d2ceb7b8ec9 107 return 1;
omidece2035 0:9d2ceb7b8ec9 108 }
omidece2035 0:9d2ceb7b8ec9 109
omidece2035 0:9d2ceb7b8ec9 110 void controlalgorithm (void)
omidece2035 0:9d2ceb7b8ec9 111 {
omidece2035 0:9d2ceb7b8ec9 112 if (i< wt) //gives you time to balance it upon reset; this is like an initialization code for the imu;
omidece2035 0:9d2ceb7b8ec9 113 {
omidece2035 0:9d2ceb7b8ec9 114 //determine current angle using a complementary filter found online http://www.pieter-jan.com/node/11
omidece2035 0:9d2ceb7b8ec9 115 //place +z-axis up, + x-axis right, and +y-axis away from the back of the robot when robot can move left to right
omidece2035 0:9d2ceb7b8ec9 116 imu.readGyro();
omidece2035 0:9d2ceb7b8ec9 117 gyro_data= imu.gy;
omidece2035 0:9d2ceb7b8ec9 118 imu.readAccel();
omidece2035 0:9d2ceb7b8ec9 119 accelerometer_angle = atan2(imu.ax, imu.az) * 180 / 3.14159; //angle in degrees; positive angle is CCW, negative angle is CW; this gives the angle as determined by the accelerometer
omidece2035 0:9d2ceb7b8ec9 120 current_angle = 0.98 * (current_angle + gyro_data * dt) + 0.02 * (accelerometer_angle);
omidece2035 0:9d2ceb7b8ec9 121 i=i+1;
omidece2035 0:9d2ceb7b8ec9 122 }
omidece2035 0:9d2ceb7b8ec9 123
omidece2035 0:9d2ceb7b8ec9 124 //if the SCALAR has been calculated and the initilization complete, start controlling
omidece2035 0:9d2ceb7b8ec9 125 if ( ready ==1 )
omidece2035 0:9d2ceb7b8ec9 126 {
omidece2035 0:9d2ceb7b8ec9 127 imu.readGyro();
omidece2035 0:9d2ceb7b8ec9 128 gyro_data= imu.gy; //maybe negative?? I don't think so
omidece2035 0:9d2ceb7b8ec9 129 imu.readAccel();
omidece2035 0:9d2ceb7b8ec9 130 accelerometer_angle = atan2(imu.ax, imu.az) * 180 / 3.14159; //angle in degrees; positive angle is CCW, negative angle is CW
omidece2035 0:9d2ceb7b8ec9 131 current_angle = 0.98 * (current_angle + gyro_data * dt) + 0.02 * (accelerometer_angle);
omidece2035 0:9d2ceb7b8ec9 132
omidece2035 0:9d2ceb7b8ec9 133 //maybe take into account integral windup
omidece2035 0:9d2ceb7b8ec9 134 Error = setpoint - current_angle; //determine the difference in angle
omidece2035 0:9d2ceb7b8ec9 135 integral = integral + (Error*dt); //Forward Euler approximation
omidece2035 0:9d2ceb7b8ec9 136 derivative = (Error - previous_error) / dt; //change wrt time
omidece2035 0:9d2ceb7b8ec9 137 previous_error = Error;
omidece2035 0:9d2ceb7b8ec9 138 actuator_input = ( (Error*kp) + (integral*ki) + (derivative*kd) ) / SCALAR; //we should scale this???? and write in terms of a PWM value [-1,1]; be careful, SCALAR is negative
omidece2035 0:9d2ceb7b8ec9 139 MotorDriver(actuator_input);
omidece2035 0:9d2ceb7b8ec9 140 }
omidece2035 0:9d2ceb7b8ec9 141
omidece2035 0:9d2ceb7b8ec9 142 }
omidece2035 0:9d2ceb7b8ec9 143
omidece2035 0:9d2ceb7b8ec9 144 void debugout (void)
omidece2035 0:9d2ceb7b8ec9 145 {
omidece2035 0:9d2ceb7b8ec9 146 //pc.printf ("setpoint is %f\n", setpoint);
omidece2035 0:9d2ceb7b8ec9 147 pc.printf ("actuator_input is %f\n", actuator_input);
omidece2035 0:9d2ceb7b8ec9 148 //pc.printf("Speed is: %f, angle is: %f\n\n",speed,current_angle);
omidece2035 0:9d2ceb7b8ec9 149 }
omidece2035 0:9d2ceb7b8ec9 150
omidece2035 0:9d2ceb7b8ec9 151 main()
omidece2035 0:9d2ceb7b8ec9 152 {
omidece2035 0:9d2ceb7b8ec9 153 pc.printf("starting...\n");
omidece2035 0:9d2ceb7b8ec9 154 uint16_t status = imu.begin();
omidece2035 0:9d2ceb7b8ec9 155 pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status);
omidece2035 0:9d2ceb7b8ec9 156 pc.printf("Should be 0x49D4\n\n");
omidece2035 0:9d2ceb7b8ec9 157
omidece2035 0:9d2ceb7b8ec9 158 SCALAR= calcSCALAR(maximum_angle); //scale the actuator inputl depends on lp, kd, and ki, and maximum angle allowed
omidece2035 0:9d2ceb7b8ec9 159 control.attach(&controlalgorithm, dt); //check frequency, 100 Hz
omidece2035 0:9d2ceb7b8ec9 160 if (debug==1)
omidece2035 0:9d2ceb7b8ec9 161 {
omidece2035 0:9d2ceb7b8ec9 162 db.attach(&debugout, 0.1);
omidece2035 0:9d2ceb7b8ec9 163 }
omidece2035 0:9d2ceb7b8ec9 164
omidece2035 0:9d2ceb7b8ec9 165 while(1)
omidece2035 0:9d2ceb7b8ec9 166 {
omidece2035 0:9d2ceb7b8ec9 167 //put some calcs here if needed; need 0.01 seconds at least
omidece2035 0:9d2ceb7b8ec9 168 if (i==wt)
omidece2035 0:9d2ceb7b8ec9 169 {
omidece2035 0:9d2ceb7b8ec9 170 // setpoint=current_angle / 1000; //take first 1000 samples for current angle then avg. it
omidece2035 0:9d2ceb7b8ec9 171 setpoint=current_angle;
omidece2035 0:9d2ceb7b8ec9 172 ready=1;
omidece2035 0:9d2ceb7b8ec9 173 i=i+1;
omidece2035 0:9d2ceb7b8ec9 174 pc.printf ("i is %d in top if statement\n\n\n\n", i);
omidece2035 0:9d2ceb7b8ec9 175 }
omidece2035 0:9d2ceb7b8ec9 176
omidece2035 0:9d2ceb7b8ec9 177 //if (debug == 1 && i>20 )
omidece2035 0:9d2ceb7b8ec9 178 // {
omidece2035 0:9d2ceb7b8ec9 179 // //probably need some kind of wait statement; will a wait statement affect the timer interrupt? no, i checked
omidece2035 0:9d2ceb7b8ec9 180 // pc.printf("accel: x: %f y: %f z: %f\n",imu.ax,imu.ay,imu.az);
omidece2035 0:9d2ceb7b8ec9 181 // pc.printf("gyro: x: %f y: %f z: %f\n",imu.gx,imu.gy,imu.gz);
omidece2035 0:9d2ceb7b8ec9 182 // pc.printf ("current_angle is %f\n", current_angle);
omidece2035 0:9d2ceb7b8ec9 183 // pc.printf ("setpoint is %f\n", setpoint);
omidece2035 0:9d2ceb7b8ec9 184 // pc.printf ("accelerometer_angle is %f\n", accelerometer_angle);
omidece2035 0:9d2ceb7b8ec9 185 // pc.printf ("gyro_angle is %f\n", gyro_angle);
omidece2035 0:9d2ceb7b8ec9 186 // pc.printf ("actuator_input is %f\n", actuator_input);
omidece2035 0:9d2ceb7b8ec9 187 // pc.printf ("SCALAR is %f \n", SCALAR);
omidece2035 0:9d2ceb7b8ec9 188 // pc.printf ("i is %d\n\n\n\n", i);
omidece2035 0:9d2ceb7b8ec9 189 // wait(2);
omidece2035 0:9d2ceb7b8ec9 190 // }
omidece2035 0:9d2ceb7b8ec9 191 }
omidece2035 0:9d2ceb7b8ec9 192 }