Biorobotics
/
Controller
Controller futhers (PI)
main.cpp@12:614f1439e679, 2015-10-28 (annotated)
- Committer:
- NickDGreg
- Date:
- Wed Oct 28 10:02:33 2015 +0000
- Revision:
- 12:614f1439e679
- Parent:
- 11:e2b2ea8a3be8
pressing both buttons moves motor two one way and then the other. It does not work as a combination moving left and right as well
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yohoo15 | 0:2b481a8db14f | 1 | #include "mbed.h" |
yohoo15 | 0:2b481a8db14f | 2 | #include "QEI.h" |
NickDGreg | 12:614f1439e679 | 3 | #include <math.h> |
yohoo15 | 0:2b481a8db14f | 4 | |
yohoo15 | 0:2b481a8db14f | 5 | Serial pc(USBTX, USBRX); |
NickDGreg | 10:3796b6a6c239 | 6 | //QEI wheel_one (PTC10, PTC11, NC, 624); // Pin for counting (analog in) |
NickDGreg | 12:614f1439e679 | 7 | QEI wheel_one (PTD3, PTD1, NC, 624); |
NickDGreg | 10:3796b6a6c239 | 8 | QEI wheel_two (PTD0, PTD2, NC, 624); |
yohoo15 | 0:2b481a8db14f | 9 | |
NickDGreg | 12:614f1439e679 | 10 | DigitalOut led_green(LED_GREEN); |
NickDGreg | 12:614f1439e679 | 11 | DigitalOut led_red(LED_RED); |
NickDGreg | 12:614f1439e679 | 12 | |
yohoo15 | 0:2b481a8db14f | 13 | // Define pin for motor control |
NickDGreg | 9:ea9d35838861 | 14 | DigitalOut directionPin_one(D4); |
NickDGreg | 9:ea9d35838861 | 15 | PwmOut PWM_one(D5); |
NickDGreg | 9:ea9d35838861 | 16 | |
NickDGreg | 10:3796b6a6c239 | 17 | PwmOut PWM_two(D6); |
NickDGreg | 10:3796b6a6c239 | 18 | DigitalOut directionPin_two(D7); |
NickDGreg | 10:3796b6a6c239 | 19 | |
yohoo15 | 7:6006a473ea0b | 20 | DigitalIn buttonccw(PTA4); |
yohoo15 | 7:6006a473ea0b | 21 | DigitalIn buttoncw(PTC6); |
yohoo15 | 0:2b481a8db14f | 22 | |
yohoo15 | 0:2b481a8db14f | 23 | // define ticker |
yohoo15 | 0:2b481a8db14f | 24 | Ticker aansturen; |
yohoo15 | 1:71617831bc4e | 25 | Ticker Printen; |
yohoo15 | 0:2b481a8db14f | 26 | |
yohoo15 | 7:6006a473ea0b | 27 | // define rotation direction and begin possition |
yohoo15 | 0:2b481a8db14f | 28 | const int cw = 1; |
yohoo15 | 0:2b481a8db14f | 29 | const int ccw = 0; |
NickDGreg | 10:3796b6a6c239 | 30 | double setpoint_one = 0; //setpoint is in pulses |
NickDGreg | 10:3796b6a6c239 | 31 | double setpoint_two = 0; //"" "" |
yohoo15 | 7:6006a473ea0b | 32 | |
yohoo15 | 7:6006a473ea0b | 33 | // saying buttons are not pressed |
yohoo15 | 7:6006a473ea0b | 34 | const int Buttoncw_pressed = 0; |
yohoo15 | 7:6006a473ea0b | 35 | const int Buttonccw_pressed = 0; |
yohoo15 | 7:6006a473ea0b | 36 | |
yohoo15 | 0:2b481a8db14f | 37 | |
yohoo15 | 3:4abcd40682fa | 38 | // Controller gain proportional and intergrator |
yohoo15 | 1:71617831bc4e | 39 | const double motor1_Kp = 5; // more or les random number. |
yohoo15 | 8:998f4575089b | 40 | const double motor1_Ki = 0; |
yohoo15 | 6:8ab07cce3098 | 41 | const double M1_timestep = 0.01; // reason ticker works with 100 Hz. |
yohoo15 | 3:4abcd40682fa | 42 | double motor1_error_integraal = 0; // initial value of integral error |
yohoo15 | 0:2b481a8db14f | 43 | |
yohoo15 | 6:8ab07cce3098 | 44 | |
yohoo15 | 7:6006a473ea0b | 45 | // Defining pulses per revolution (calculating pulses to rotations in degree.) |
yohoo15 | 6:8ab07cce3098 | 46 | const double pulses_per_revolution = 4200 ;//8400 counts is aangegeven op de motor for x4. 10 - 30 counts oveshoot. for moter 1(tape)! Motor 2 almost the same(nice) |
yohoo15 | 7:6006a473ea0b | 47 | /* |
yohoo15 | 8:998f4575089b | 48 | double Rotation = -2; // rotation |
yohoo15 | 6:8ab07cce3098 | 49 | double movement = Rotation * pulses_per_revolution; // times 360 to make Rotations degree. |
yohoo15 | 7:6006a473ea0b | 50 | */ |
yohoo15 | 0:2b481a8db14f | 51 | |
yohoo15 | 5:d1ab07fd3355 | 52 | // defining flags |
yohoo15 | 5:d1ab07fd3355 | 53 | volatile bool flag_motor = false; |
yohoo15 | 5:d1ab07fd3355 | 54 | volatile bool flag_pcprint = false; |
yohoo15 | 0:2b481a8db14f | 55 | |
yohoo15 | 6:8ab07cce3098 | 56 | // making function flags. |
yohoo15 | 5:d1ab07fd3355 | 57 | void Go_flag_motor() |
yohoo15 | 5:d1ab07fd3355 | 58 | { |
yohoo15 | 5:d1ab07fd3355 | 59 | flag_motor = true; |
yohoo15 | 5:d1ab07fd3355 | 60 | } |
yohoo15 | 5:d1ab07fd3355 | 61 | |
yohoo15 | 5:d1ab07fd3355 | 62 | void Go_flag_pcprint() |
yohoo15 | 5:d1ab07fd3355 | 63 | { |
yohoo15 | 5:d1ab07fd3355 | 64 | flag_pcprint = true; |
yohoo15 | 5:d1ab07fd3355 | 65 | } |
yohoo15 | 0:2b481a8db14f | 66 | |
yohoo15 | 7:6006a473ea0b | 67 | // To make a new setpoint |
NickDGreg | 9:ea9d35838861 | 68 | double making_setpoint(double direction, double setpoint){ |
yohoo15 | 7:6006a473ea0b | 69 | if ( cw == direction){ |
NickDGreg | 9:ea9d35838861 | 70 | setpoint = setpoint + (pulses_per_revolution/400000); |
yohoo15 | 7:6006a473ea0b | 71 | } |
yohoo15 | 7:6006a473ea0b | 72 | if ( ccw == direction){ |
NickDGreg | 9:ea9d35838861 | 73 | setpoint = setpoint - (pulses_per_revolution/400000); |
yohoo15 | 7:6006a473ea0b | 74 | } |
yohoo15 | 7:6006a473ea0b | 75 | return(setpoint); |
yohoo15 | 8:998f4575089b | 76 | |
yohoo15 | 7:6006a473ea0b | 77 | } |
yohoo15 | 7:6006a473ea0b | 78 | |
yohoo15 | 0:2b481a8db14f | 79 | // Reusable P controller |
yohoo15 | 3:4abcd40682fa | 80 | double PI(double error, const double Kp, const double Ki, double Ts, double &e_int) |
yohoo15 | 6:8ab07cce3098 | 81 | { |
yohoo15 | 6:8ab07cce3098 | 82 | |
yohoo15 | 6:8ab07cce3098 | 83 | e_int = e_int + Ts * error; |
yohoo15 | 6:8ab07cce3098 | 84 | |
yohoo15 | 6:8ab07cce3098 | 85 | double PI_output = (Kp * error) + (Ki * e_int); |
yohoo15 | 3:4abcd40682fa | 86 | return PI_output; |
yohoo15 | 0:2b481a8db14f | 87 | } |
yohoo15 | 0:2b481a8db14f | 88 | // Next task, measure the error and apply the output to the plant |
NickDGreg | 9:ea9d35838861 | 89 | void motor1_Controller(DigitalOut directionPin, PwmOut PWM, QEI &wheel, double setpoint) |
yohoo15 | 0:2b481a8db14f | 90 | { |
yohoo15 | 7:6006a473ea0b | 91 | double reference = setpoint; // setpoint is in pulses |
yohoo15 | 0:2b481a8db14f | 92 | double position = wheel.getPulses(); |
yohoo15 | 6:8ab07cce3098 | 93 | double error_pulses = (reference - position); // calculate the error in pulses |
yohoo15 | 6:8ab07cce3098 | 94 | double error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotaions the motor has to turn |
yohoo15 | 6:8ab07cce3098 | 95 | |
yohoo15 | 6:8ab07cce3098 | 96 | double output = abs(PI( error_rotation, motor1_Kp, motor1_Ki, M1_timestep, motor1_error_integraal )); |
yohoo15 | 0:2b481a8db14f | 97 | |
yohoo15 | 6:8ab07cce3098 | 98 | if(error_pulses > 0) { |
yohoo15 | 0:2b481a8db14f | 99 | directionPin.write(cw); |
yohoo15 | 6:8ab07cce3098 | 100 | |
yohoo15 | 6:8ab07cce3098 | 101 | } |
yohoo15 | 6:8ab07cce3098 | 102 | else if(error_pulses < 0) { |
yohoo15 | 6:8ab07cce3098 | 103 | directionPin.write(ccw); |
yohoo15 | 0:2b481a8db14f | 104 | } |
yohoo15 | 6:8ab07cce3098 | 105 | else{ |
yohoo15 | 6:8ab07cce3098 | 106 | output = 0; |
yohoo15 | 6:8ab07cce3098 | 107 | } |
NickDGreg | 10:3796b6a6c239 | 108 | |
NickDGreg | 10:3796b6a6c239 | 109 | PWM.write(output); // out of the if loop due to abs output |
yohoo15 | 0:2b481a8db14f | 110 | |
yohoo15 | 0:2b481a8db14f | 111 | |
yohoo15 | 0:2b481a8db14f | 112 | } |
yohoo15 | 0:2b481a8db14f | 113 | |
NickDGreg | 12:614f1439e679 | 114 | //control for the keypress |
NickDGreg | 12:614f1439e679 | 115 | void motor2_Controller(DigitalOut directionPin, PwmOut PWM, QEI &wheel, double setpoint) |
NickDGreg | 12:614f1439e679 | 116 | { |
NickDGreg | 12:614f1439e679 | 117 | double reference = setpoint; // setpoint is in pulses |
NickDGreg | 12:614f1439e679 | 118 | double position = wheel_two.getPulses(); |
NickDGreg | 12:614f1439e679 | 119 | double error_pulses = (reference - position); // calculate the error in pulses |
NickDGreg | 12:614f1439e679 | 120 | double error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotaions the motor has to turn |
NickDGreg | 12:614f1439e679 | 121 | |
NickDGreg | 12:614f1439e679 | 122 | while (fabs(error_pulses) > 40) |
NickDGreg | 12:614f1439e679 | 123 | { |
NickDGreg | 12:614f1439e679 | 124 | //kijken = wheel_two.getPulses()/pulses_per_revolution; |
NickDGreg | 12:614f1439e679 | 125 | |
NickDGreg | 12:614f1439e679 | 126 | double output = abs(PI( error_rotation, motor1_Kp, motor1_Ki, M1_timestep, motor1_error_integraal )); |
NickDGreg | 12:614f1439e679 | 127 | |
NickDGreg | 12:614f1439e679 | 128 | if(error_pulses > 0) { |
NickDGreg | 12:614f1439e679 | 129 | directionPin.write(cw); |
NickDGreg | 12:614f1439e679 | 130 | |
NickDGreg | 12:614f1439e679 | 131 | } |
NickDGreg | 12:614f1439e679 | 132 | else if(error_pulses < 0) { |
NickDGreg | 12:614f1439e679 | 133 | directionPin.write(ccw); |
NickDGreg | 12:614f1439e679 | 134 | } |
NickDGreg | 12:614f1439e679 | 135 | else{ |
NickDGreg | 12:614f1439e679 | 136 | output = 0; |
NickDGreg | 12:614f1439e679 | 137 | } |
NickDGreg | 12:614f1439e679 | 138 | |
NickDGreg | 12:614f1439e679 | 139 | PWM.write(output); // out of the if loop due to abs output |
NickDGreg | 12:614f1439e679 | 140 | |
NickDGreg | 12:614f1439e679 | 141 | position = wheel.getPulses(); |
NickDGreg | 12:614f1439e679 | 142 | error_pulses = (reference - position); |
NickDGreg | 12:614f1439e679 | 143 | error_rotation = error_pulses / pulses_per_revolution; |
NickDGreg | 12:614f1439e679 | 144 | } |
yohoo15 | 0:2b481a8db14f | 145 | |
NickDGreg | 12:614f1439e679 | 146 | } |
NickDGreg | 12:614f1439e679 | 147 | |
NickDGreg | 12:614f1439e679 | 148 | |
NickDGreg | 12:614f1439e679 | 149 | void counts_showing(double setpoint, QEI &wheel) |
yohoo15 | 6:8ab07cce3098 | 150 | { |
NickDGreg | 12:614f1439e679 | 151 | |
NickDGreg | 12:614f1439e679 | 152 | //double error_pulses = setpoint - wheel_one.getPulses(); |
NickDGreg | 10:3796b6a6c239 | 153 | //double kijken = wheel_one.getPulses()/pulses_per_revolution; |
NickDGreg | 12:614f1439e679 | 154 | //pc.printf("ref %.2f rounds%.2f error pulses%.2f \n",setpoint_one/pulses_per_revolution,kijken, error_pulses); |
NickDGreg | 12:614f1439e679 | 155 | double position = wheel.getPulses(); |
NickDGreg | 12:614f1439e679 | 156 | double error_pulses = setpoint - position; |
NickDGreg | 12:614f1439e679 | 157 | pc.printf("ref %.2f position%.2f error_pulses%.2f \n",setpoint, position, error_pulses); |
yohoo15 | 6:8ab07cce3098 | 158 | |
yohoo15 | 6:8ab07cce3098 | 159 | } |
yohoo15 | 0:2b481a8db14f | 160 | |
yohoo15 | 0:2b481a8db14f | 161 | int main() |
yohoo15 | 0:2b481a8db14f | 162 | { |
yohoo15 | 6:8ab07cce3098 | 163 | aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning |
yohoo15 | 5:d1ab07fd3355 | 164 | Printen.attach(&Go_flag_pcprint,0.1f); // 10 Hz // printstatement here because printing cost to much time. the motor void wouldn't be completed |
yohoo15 | 5:d1ab07fd3355 | 165 | while( 1 ) { |
yohoo15 | 7:6006a473ea0b | 166 | // to make the motor move |
NickDGreg | 12:614f1439e679 | 167 | led_green = 0; |
yohoo15 | 5:d1ab07fd3355 | 168 | if(flag_motor) { |
yohoo15 | 5:d1ab07fd3355 | 169 | flag_motor = false; |
NickDGreg | 10:3796b6a6c239 | 170 | //motor1_Controller(directionPin_one, PWM_one, wheel_one, setpoint_one); |
NickDGreg | 12:614f1439e679 | 171 | //motor1_Controller(directionPin_two, PWM_two, wheel_two, setpoint_two); |
yohoo15 | 5:d1ab07fd3355 | 172 | } |
yohoo15 | 5:d1ab07fd3355 | 173 | |
yohoo15 | 5:d1ab07fd3355 | 174 | if(flag_pcprint) { |
yohoo15 | 5:d1ab07fd3355 | 175 | flag_pcprint = false; |
NickDGreg | 12:614f1439e679 | 176 | counts_showing(setpoint_two, wheel_two); |
yohoo15 | 5:d1ab07fd3355 | 177 | } |
yohoo15 | 5:d1ab07fd3355 | 178 | |
NickDGreg | 12:614f1439e679 | 179 | led_green = 1; |
NickDGreg | 11:e2b2ea8a3be8 | 180 | // Pussing buttons to get input s0ignal |
NickDGreg | 12:614f1439e679 | 181 | /* |
NickDGreg | 12:614f1439e679 | 182 | if(buttoncw.read() == Buttoncw_pressed and buttonccw.read() == Buttonccw_pressed) |
NickDGreg | 11:e2b2ea8a3be8 | 183 | { |
NickDGreg | 12:614f1439e679 | 184 | led_red = 0; |
NickDGreg | 12:614f1439e679 | 185 | |
NickDGreg | 12:614f1439e679 | 186 | |
NickDGreg | 12:614f1439e679 | 187 | setpoint_two = setpoint_two + 1050; |
NickDGreg | 12:614f1439e679 | 188 | motor2_Controller(directionPin_two, PWM_two, wheel_two, setpoint_two); |
NickDGreg | 12:614f1439e679 | 189 | setpoint_two = setpoint_two - 1050; |
NickDGreg | 12:614f1439e679 | 190 | motor2_Controller(directionPin_two, PWM_two, wheel_two, setpoint_two); |
NickDGreg | 12:614f1439e679 | 191 | |
NickDGreg | 12:614f1439e679 | 192 | led_green = 0; |
NickDGreg | 12:614f1439e679 | 193 | wait(1); |
NickDGreg | 12:614f1439e679 | 194 | led_green = 1; |
NickDGreg | 12:614f1439e679 | 195 | |
NickDGreg | 12:614f1439e679 | 196 | }*/ |
NickDGreg | 12:614f1439e679 | 197 | //led_red = 1; |
NickDGreg | 10:3796b6a6c239 | 198 | if(buttoncw.read() == Buttoncw_pressed) |
NickDGreg | 10:3796b6a6c239 | 199 | { |
NickDGreg | 12:614f1439e679 | 200 | setpoint_one = making_setpoint(cw, setpoint_one); |
NickDGreg | 12:614f1439e679 | 201 | //setpoint_two = making_setpoint(ccw, setpoint_two); |
yohoo15 | 7:6006a473ea0b | 202 | } |
NickDGreg | 10:3796b6a6c239 | 203 | |
NickDGreg | 10:3796b6a6c239 | 204 | if(buttonccw.read() == Buttonccw_pressed) |
NickDGreg | 10:3796b6a6c239 | 205 | { |
NickDGreg | 12:614f1439e679 | 206 | setpoint_one = making_setpoint(ccw, setpoint_one); |
NickDGreg | 12:614f1439e679 | 207 | //setpoint_two = making_setpoint(cw, setpoint_two); |
yohoo15 | 7:6006a473ea0b | 208 | } |
yohoo15 | 7:6006a473ea0b | 209 | |
yohoo15 | 5:d1ab07fd3355 | 210 | } |
yohoo15 | 0:2b481a8db14f | 211 | } |
yohoo15 | 0:2b481a8db14f | 212 | |
yohoo15 | 0:2b481a8db14f | 213 | |
yohoo15 | 0:2b481a8db14f | 214 |