Sem Bertelink / Mbed 2 deprecated MOD09_Biorobotics_Groep_14lol

lololololol

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@23:78898ddfb103, 2019-10-21 (annotated)

Committer:
Hendrikvg
Date:
Mon Oct 21 10:14:44 2019 +0000
Revision:
23:78898ddfb103
Parent:
22:6cc93216b323
Child:
24:a9ec9b836fd9
motor 2 functionaliteit ingebouwd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Hendrikvg 17:cacf9e75eda7 1 #include "QEI.h"
Hendrikvg 15:80b3ac2b8448 2 #include "mbed.h"
Hendrikvg 20:ac1b4ffa3323 3 #include "BiQuad.h"
Hendrikvg 14:20f11bb58244 4 #include "FastPWM.h"
Hendrikvg 17:cacf9e75eda7 5 #include "HIDScope.h"
Hendrikvg 16:40183eeadb6d 6 #include "MODSERIAL.h"
Hendrikvg 9:12b9865e7373 7
Hendrikvg 21:394a7a1deb73 8 // Pins
Hendrikvg 21:394a7a1deb73 9 MODSERIAL pc(USBTX, USBRX);
Hendrikvg 21:394a7a1deb73 10 InterruptIn stepresponse(D0);
Hendrikvg 21:394a7a1deb73 11 FastPWM PWM_motor_1(D6);
Hendrikvg 23:78898ddfb103 12 FastPWM PWM_motor_2(D5);
Hendrikvg 20:ac1b4ffa3323 13 DigitalOut direction_motor_1(D7);
Hendrikvg 23:78898ddfb103 14 DigitalOut direction_motor_2(D4);
Hendrikvg 21:394a7a1deb73 15 InterruptIn button_1(SW2);
Hendrikvg 21:394a7a1deb73 16 InterruptIn button_2(SW3);
Hendrikvg 17:cacf9e75eda7 17
Hendrikvg 15:80b3ac2b8448 18 // variables
Hendrikvg 21:394a7a1deb73 19 Ticker TickerStateMachine;
Hendrikvg 23:78898ddfb103 20 Ticker motor_control;
Hendrikvg 23:78898ddfb103 21 Ticker write_scope;
Hendrikvg 21:394a7a1deb73 22 Timer sinus_time;
Hendrikvg 21:394a7a1deb73 23 enum states {start, motor_calibration, demo_mode, emg_calibration, vertical_movement, horizontal_movement};
Hendrikvg 21:394a7a1deb73 24 states CurrentState = start;
Hendrikvg 21:394a7a1deb73 25 bool StateChanged = true;
Hendrikvg 21:394a7a1deb73 26 bool demo = false;
Hendrikvg 21:394a7a1deb73 27 bool emg = false;
Hendrikvg 21:394a7a1deb73 28 bool next = false;
Hendrikvg 21:394a7a1deb73 29 HIDScope scope(3);
Hendrikvg 23:78898ddfb103 30 QEI encoder_1(D10,D11,NC,8400,QEI::X4_ENCODING);
Hendrikvg 23:78898ddfb103 31 QEI encoder_2(D12,D13,NC,8400,QEI::X4_ENCODING);
Hendrikvg 22:6cc93216b323 32 volatile double theta_1;
Hendrikvg 23:78898ddfb103 33 //volatile float theta_error_1;
Hendrikvg 22:6cc93216b323 34 volatile float theta_reference_1;
Hendrikvg 23:78898ddfb103 35 volatile double theta_2;
Hendrikvg 23:78898ddfb103 36 //volatile float theta_error_2;
Hendrikvg 23:78898ddfb103 37 volatile float theta_reference_2;
Hendrikvg 21:394a7a1deb73 38 float Ts = 0.001;
Hendrikvg 23:78898ddfb103 39 float Kp;
Hendrikvg 23:78898ddfb103 40 float Ki;
Hendrikvg 23:78898ddfb103 41 float Kd;
Hendrikvg 20:ac1b4ffa3323 42
Hendrikvg 21:394a7a1deb73 43 // functies
Hendrikvg 22:6cc93216b323 44 float CalculateError(float theta_reference,float theta)
Hendrikvg 21:394a7a1deb73 45 {
Hendrikvg 22:6cc93216b323 46 float theta_error = theta_reference-theta;
Hendrikvg 21:394a7a1deb73 47 return theta_error;
Hendrikvg 21:394a7a1deb73 48 }
Hendrikvg 20:ac1b4ffa3323 49
Hendrikvg 23:78898ddfb103 50 float Controller(float theta_error, bool motor)
Hendrikvg 21:394a7a1deb73 51 {
Hendrikvg 23:78898ddfb103 52 if (motor == false)
Hendrikvg 23:78898ddfb103 53 {
Hendrikvg 23:78898ddfb103 54 float K = 1;
Hendrikvg 23:78898ddfb103 55 float ti = 0.1;
Hendrikvg 23:78898ddfb103 56 float td = 10;
Hendrikvg 23:78898ddfb103 57 Kp = K*(1+td/ti);
Hendrikvg 23:78898ddfb103 58 Ki = K/ti;
Hendrikvg 23:78898ddfb103 59 Kd = K*td;
Hendrikvg 23:78898ddfb103 60 }
Hendrikvg 23:78898ddfb103 61 else
Hendrikvg 23:78898ddfb103 62 {
Hendrikvg 23:78898ddfb103 63 float K = 1;
Hendrikvg 23:78898ddfb103 64 float ti = 0.1;
Hendrikvg 23:78898ddfb103 65 float td = 10;
Hendrikvg 23:78898ddfb103 66 Kp = K*(1+td/ti);
Hendrikvg 23:78898ddfb103 67 Ki = K/ti;
Hendrikvg 23:78898ddfb103 68 Kd = K*td;
Hendrikvg 23:78898ddfb103 69 }
Hendrikvg 21:394a7a1deb73 70 static float error_integral = 0;
Hendrikvg 21:394a7a1deb73 71 static float error_prev = 0;
Hendrikvg 21:394a7a1deb73 72 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
Hendrikvg 20:ac1b4ffa3323 73
Hendrikvg 20:ac1b4ffa3323 74 // Proportional part:
Hendrikvg 20:ac1b4ffa3323 75 float torque_p = Kp * theta_error;
Hendrikvg 20:ac1b4ffa3323 76
Hendrikvg 20:ac1b4ffa3323 77 // Integral part:
Hendrikvg 21:394a7a1deb73 78 error_integral = error_integral + theta_error * Ts;
Hendrikvg 21:394a7a1deb73 79 float torque_i = Ki * error_integral;
Hendrikvg 20:ac1b4ffa3323 80
Hendrikvg 20:ac1b4ffa3323 81 // Derivative part:
Hendrikvg 21:394a7a1deb73 82 float error_derivative = (theta_error - error_prev)/Ts;
Hendrikvg 21:394a7a1deb73 83 float filtered_error_derivative = LowPassFilter.step(error_derivative);
Hendrikvg 21:394a7a1deb73 84 float torque_d = Kd * filtered_error_derivative;
Hendrikvg 21:394a7a1deb73 85 error_prev = theta_error;
Hendrikvg 20:ac1b4ffa3323 86
Hendrikvg 20:ac1b4ffa3323 87 // Sum all parts and return it
Hendrikvg 21:394a7a1deb73 88 float torque = torque_p + torque_i + torque_d;
Hendrikvg 21:394a7a1deb73 89 return torque;
Hendrikvg 21:394a7a1deb73 90 }
Hendrikvg 16:40183eeadb6d 91
Hendrikvg 22:6cc93216b323 92 void CalculateDirectionMotor()
Hendrikvg 21:394a7a1deb73 93 {
Hendrikvg 23:78898ddfb103 94 direction_motor_1 = Controller(CalculateError(theta_reference_1,theta_1),0) <= 0.0f;
Hendrikvg 23:78898ddfb103 95 direction_motor_2 = Controller(CalculateError(theta_reference_2,theta_2),1) <= 0.0f;
Hendrikvg 21:394a7a1deb73 96 }
Hendrikvg 20:ac1b4ffa3323 97
Hendrikvg 21:394a7a1deb73 98 void WriteScope()
Hendrikvg 21:394a7a1deb73 99 {
Hendrikvg 22:6cc93216b323 100 scope.set(0,theta_1);
Hendrikvg 22:6cc93216b323 101 scope.set(1,CalculateError(theta_reference_1,theta_1));
Hendrikvg 22:6cc93216b323 102 scope.set(2,theta_reference_1);
Hendrikvg 21:394a7a1deb73 103 scope.send();
Hendrikvg 21:394a7a1deb73 104 }
Hendrikvg 16:40183eeadb6d 105
Hendrikvg 23:78898ddfb103 106 void ReadEncoder()
Hendrikvg 23:78898ddfb103 107 {
Hendrikvg 23:78898ddfb103 108 theta_1 = ((360.0f/64.0f)*(float)encoder_1.getPulses())/131.25f; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360.
Hendrikvg 23:78898ddfb103 109 theta_2 = ((360.0f/64.0f)*(float)encoder_2.getPulses())/131.25f;
Hendrikvg 23:78898ddfb103 110 }
Hendrikvg 16:40183eeadb6d 111
Hendrikvg 23:78898ddfb103 112 void MotorControl()
Hendrikvg 23:78898ddfb103 113 {
Hendrikvg 21:394a7a1deb73 114 ReadEncoder();
Hendrikvg 23:78898ddfb103 115 theta_reference_1 = 360*sin(0.1f*sinus_time.read()*2*3.14f); // voor test, moet weg in eindscript
Hendrikvg 22:6cc93216b323 116 CalculateDirectionMotor();
Hendrikvg 23:78898ddfb103 117 PWM_motor_1.write(fabs(Controller(CalculateError(theta_reference_1,theta_1),0)/360.0f));
Hendrikvg 23:78898ddfb103 118 PWM_motor_2.write(fabs(Controller(CalculateError(theta_reference_2,theta_2),1)/360.0f));
Hendrikvg 21:394a7a1deb73 119 }
Hendrikvg 21:394a7a1deb73 120
Hendrikvg 21:394a7a1deb73 121 void go_next_1()
Hendrikvg 21:394a7a1deb73 122 {
Hendrikvg 21:394a7a1deb73 123 demo = !demo;
Hendrikvg 21:394a7a1deb73 124 }
Hendrikvg 21:394a7a1deb73 125
Hendrikvg 21:394a7a1deb73 126 void go_next_2()
Hendrikvg 21:394a7a1deb73 127 {
Hendrikvg 21:394a7a1deb73 128 emg = !emg;
Hendrikvg 21:394a7a1deb73 129 next = emg;
Hendrikvg 21:394a7a1deb73 130 }
Hendrikvg 21:394a7a1deb73 131
Hendrikvg 21:394a7a1deb73 132 void while_start()
Hendrikvg 21:394a7a1deb73 133 {
Hendrikvg 21:394a7a1deb73 134 // Do startup stuff
Hendrikvg 21:394a7a1deb73 135 CurrentState = motor_calibration;
Hendrikvg 21:394a7a1deb73 136 StateChanged = true;
Hendrikvg 21:394a7a1deb73 137 }
Hendrikvg 21:394a7a1deb73 138
Hendrikvg 21:394a7a1deb73 139 void while_motor_calibration()
Hendrikvg 21:394a7a1deb73 140 {
Hendrikvg 21:394a7a1deb73 141 // Do motor calibration stuff
Hendrikvg 21:394a7a1deb73 142 if (demo) // bool aanmaken voor demo (switch oid aanmaken)
Hendrikvg 21:394a7a1deb73 143 {
Hendrikvg 21:394a7a1deb73 144 CurrentState = demo_mode;
Hendrikvg 21:394a7a1deb73 145 StateChanged = true;
Hendrikvg 21:394a7a1deb73 146 }
Hendrikvg 21:394a7a1deb73 147 if (emg) // bool aanmaken voor EMG (switch oid aanmaken)
Hendrikvg 21:394a7a1deb73 148 {
Hendrikvg 21:394a7a1deb73 149 CurrentState = emg_calibration;
Hendrikvg 21:394a7a1deb73 150 StateChanged = true;
Hendrikvg 21:394a7a1deb73 151 }
Hendrikvg 21:394a7a1deb73 152 }
Hendrikvg 21:394a7a1deb73 153
Hendrikvg 21:394a7a1deb73 154 void while_demo_mode()
Hendrikvg 21:394a7a1deb73 155 {
Hendrikvg 21:394a7a1deb73 156 // Do demo mode stuff
Hendrikvg 21:394a7a1deb73 157 if (!demo) // bool aanmaken voor demo (switch oid)
Hendrikvg 21:394a7a1deb73 158 {
Hendrikvg 21:394a7a1deb73 159 emg = true;
Hendrikvg 21:394a7a1deb73 160 }
Hendrikvg 21:394a7a1deb73 161 if (emg) // bool aanmaken voor EMG (switch oid aanmaken)
Hendrikvg 21:394a7a1deb73 162 {
Hendrikvg 21:394a7a1deb73 163 CurrentState = emg_calibration;
Hendrikvg 21:394a7a1deb73 164 StateChanged = true;
Hendrikvg 21:394a7a1deb73 165 }
Hendrikvg 21:394a7a1deb73 166 }
Hendrikvg 21:394a7a1deb73 167
Hendrikvg 21:394a7a1deb73 168 void while_emg_calibration()
Hendrikvg 21:394a7a1deb73 169 {
Hendrikvg 21:394a7a1deb73 170 // Do emg calibration stuff
Hendrikvg 21:394a7a1deb73 171 if (!emg) // bool aanmaken voor EMG (switch)
Hendrikvg 21:394a7a1deb73 172 {
Hendrikvg 21:394a7a1deb73 173 CurrentState = vertical_movement;
Hendrikvg 21:394a7a1deb73 174 StateChanged = true;
Hendrikvg 21:394a7a1deb73 175 }
Hendrikvg 21:394a7a1deb73 176 }
Hendrikvg 21:394a7a1deb73 177
Hendrikvg 21:394a7a1deb73 178 void while_vertical_movement()
Hendrikvg 21:394a7a1deb73 179 {
Hendrikvg 21:394a7a1deb73 180 // Do vertical movement stuff
Hendrikvg 21:394a7a1deb73 181 if (next) // EMG gebaseerde threshold aanmaken
Hendrikvg 21:394a7a1deb73 182 {
Hendrikvg 21:394a7a1deb73 183 CurrentState = horizontal_movement;
Hendrikvg 21:394a7a1deb73 184 StateChanged = true;
Hendrikvg 21:394a7a1deb73 185 }
Hendrikvg 21:394a7a1deb73 186 }
Hendrikvg 17:cacf9e75eda7 187
Hendrikvg 21:394a7a1deb73 188 void while_horizontal_movement()
Hendrikvg 21:394a7a1deb73 189 {
Hendrikvg 21:394a7a1deb73 190 // Do horizontal movement stuff
Hendrikvg 21:394a7a1deb73 191 if (!next) // EMG gebaseerde threshold aanmaken
Hendrikvg 21:394a7a1deb73 192 {
Hendrikvg 21:394a7a1deb73 193 CurrentState = vertical_movement;
Hendrikvg 21:394a7a1deb73 194 StateChanged = true;
Hendrikvg 21:394a7a1deb73 195 }
Hendrikvg 21:394a7a1deb73 196 }
Hendrikvg 21:394a7a1deb73 197
Hendrikvg 21:394a7a1deb73 198 void StateTransition()
Hendrikvg 21:394a7a1deb73 199 {
Hendrikvg 21:394a7a1deb73 200 if (StateChanged)
Hendrikvg 21:394a7a1deb73 201 {
Hendrikvg 21:394a7a1deb73 202 if (CurrentState == start)
Hendrikvg 21:394a7a1deb73 203 {
Hendrikvg 21:394a7a1deb73 204 pc.printf("Initiating start.\n\r");
Hendrikvg 21:394a7a1deb73 205 }
Hendrikvg 21:394a7a1deb73 206 if (CurrentState == motor_calibration)
Hendrikvg 21:394a7a1deb73 207 {
Hendrikvg 21:394a7a1deb73 208 pc.printf("Initiating motor_calibration.\n\r");
Hendrikvg 21:394a7a1deb73 209 }
Hendrikvg 21:394a7a1deb73 210 if (CurrentState == demo_mode)
Hendrikvg 21:394a7a1deb73 211 {
Hendrikvg 21:394a7a1deb73 212 pc.printf("Initiating demo_mode.\n\r");
Hendrikvg 21:394a7a1deb73 213 }
Hendrikvg 21:394a7a1deb73 214 if (CurrentState == emg_calibration)
Hendrikvg 21:394a7a1deb73 215 {
Hendrikvg 21:394a7a1deb73 216 pc.printf("Initiating emg_calibration.\n\r");
Hendrikvg 21:394a7a1deb73 217 }
Hendrikvg 21:394a7a1deb73 218 if (CurrentState == vertical_movement)
Hendrikvg 21:394a7a1deb73 219 {
Hendrikvg 21:394a7a1deb73 220 pc.printf("Initiating vertical_movement.\n\r");
Hendrikvg 21:394a7a1deb73 221 }
Hendrikvg 21:394a7a1deb73 222 if (CurrentState == horizontal_movement)
Hendrikvg 21:394a7a1deb73 223 {
Hendrikvg 21:394a7a1deb73 224 pc.printf("Initiating horizontal_movement.\n\r");
Hendrikvg 21:394a7a1deb73 225 }
Hendrikvg 21:394a7a1deb73 226 StateChanged = false;
Hendrikvg 21:394a7a1deb73 227 }
Hendrikvg 21:394a7a1deb73 228 }
Hendrikvg 21:394a7a1deb73 229
Hendrikvg 21:394a7a1deb73 230 void StateMachine()
Hendrikvg 21:394a7a1deb73 231 {
Hendrikvg 21:394a7a1deb73 232 switch(CurrentState)
Hendrikvg 21:394a7a1deb73 233 {
Hendrikvg 21:394a7a1deb73 234 case start:
Hendrikvg 21:394a7a1deb73 235 StateTransition();
Hendrikvg 21:394a7a1deb73 236 while_start();
Hendrikvg 21:394a7a1deb73 237 break;
Hendrikvg 21:394a7a1deb73 238 case motor_calibration:
Hendrikvg 21:394a7a1deb73 239 StateTransition();
Hendrikvg 21:394a7a1deb73 240 while_motor_calibration();
Hendrikvg 21:394a7a1deb73 241 break;
Hendrikvg 21:394a7a1deb73 242 case demo_mode:
Hendrikvg 21:394a7a1deb73 243 StateTransition();
Hendrikvg 21:394a7a1deb73 244 while_demo_mode();
Hendrikvg 21:394a7a1deb73 245 break;
Hendrikvg 21:394a7a1deb73 246 case emg_calibration:
Hendrikvg 21:394a7a1deb73 247 StateTransition();
Hendrikvg 21:394a7a1deb73 248 while_emg_calibration();
Hendrikvg 21:394a7a1deb73 249 break;
Hendrikvg 21:394a7a1deb73 250 case vertical_movement:
Hendrikvg 21:394a7a1deb73 251 StateTransition();
Hendrikvg 21:394a7a1deb73 252 while_vertical_movement();
Hendrikvg 21:394a7a1deb73 253 break;
Hendrikvg 21:394a7a1deb73 254 case horizontal_movement:
Hendrikvg 21:394a7a1deb73 255 StateTransition();
Hendrikvg 21:394a7a1deb73 256 while_horizontal_movement();
Hendrikvg 21:394a7a1deb73 257 break;
Hendrikvg 21:394a7a1deb73 258 default:
Hendrikvg 21:394a7a1deb73 259 pc.printf("Unknown or unimplemented state reached!\n\r");
Hendrikvg 21:394a7a1deb73 260 }
Hendrikvg 21:394a7a1deb73 261 }
Hendrikvg 21:394a7a1deb73 262
Hendrikvg 15:80b3ac2b8448 263 // main
Hendrikvg 21:394a7a1deb73 264 int main(){
RobertoO 0:67c50348f842 265 pc.baud(115200);
Hendrikvg 17:cacf9e75eda7 266 pc.printf("Hello World!\n\r");
Hendrikvg 21:394a7a1deb73 267 button_1.fall(go_next_1);
Hendrikvg 21:394a7a1deb73 268 button_2.fall(go_next_2);
Hendrikvg 21:394a7a1deb73 269 sinus_time.start();
Hendrikvg 21:394a7a1deb73 270 PWM_motor_1.period_ms(10);
Hendrikvg 23:78898ddfb103 271 motor_control.attach(&MotorControl, Ts);
Hendrikvg 23:78898ddfb103 272 write_scope.attach(&WriteScope, 0.1);
Hendrikvg 21:394a7a1deb73 273 //TickerStateMachine.attach(StateMachine,1.00f);
Hendrikvg 21:394a7a1deb73 274 while(true) {
Hendrikvg 21:394a7a1deb73 275 StateMachine();
Hendrikvg 21:394a7a1deb73 276 }
Hendrikvg 2:d9b0ebf3fcca 277 }