Sem Bertelink / Mbed 2 deprecated MOD09_Biorobotics_Groep_14lol

lololololol

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@21:394a7a1deb73, 2019-10-14 (annotated)

Committer:
Hendrikvg
Date:
Mon Oct 14 08:50:26 2019 +0000
Revision:
21:394a7a1deb73
Parent:
20:ac1b4ffa3323
Child:
22:6cc93216b323
State machine en PID controller

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