Berk Yaşar YAVUZ
/
balancebot
balance
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?
User | Revision | Line number | New 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 | } |