Sem Bertelink / Mbed 2 deprecated MOD09_Biorobotics_Groep_14lol

lololololol

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@22:6cc93216b323, 2019-10-14 (annotated)

Committer:
Hendrikvg
Date:
Mon Oct 14 13:26:46 2019 +0000
Revision:
22:6cc93216b323
Parent:
21:394a7a1deb73
Child:
23:78898ddfb103
calculate error algemener gemaakt zodat motor 2 makkelijker toegevoegd kan worden

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 22:6cc93216b323 29 volatile double theta_1;
Hendrikvg 22:6cc93216b323 30 volatile float theta_error_1;
Hendrikvg 22:6cc93216b323 31 volatile float theta_reference_1;
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 22:6cc93216b323 41 float CalculateError(float theta_reference,float theta)
Hendrikvg 21:394a7a1deb73 42 {
Hendrikvg 22:6cc93216b323 43 float 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 22:6cc93216b323 71 void CalculateDirectionMotor()
Hendrikvg 21:394a7a1deb73 72 {
Hendrikvg 22:6cc93216b323 73 direction_motor_1 = Controller(CalculateError(theta_reference_1,theta_1)) <= 0.0f;
Hendrikvg 22:6cc93216b323 74 //direction_motor_2 = Controller(CalculateError(theta_reference_2,theta_2)) <= 0.0f;
Hendrikvg 21:394a7a1deb73 75 }
Hendrikvg 20:ac1b4ffa3323 76
Hendrikvg 21:394a7a1deb73 77 void WriteScope()
Hendrikvg 21:394a7a1deb73 78 {
Hendrikvg 22:6cc93216b323 79 scope.set(0,theta_1);
Hendrikvg 21:394a7a1deb73 80 //scope.set(1,Controller(CalculateError()));
Hendrikvg 22:6cc93216b323 81 scope.set(1,CalculateError(theta_reference_1,theta_1));
Hendrikvg 22:6cc93216b323 82 scope.set(2,theta_reference_1);
Hendrikvg 21:394a7a1deb73 83 scope.send();
Hendrikvg 21:394a7a1deb73 84 }
Hendrikvg 16:40183eeadb6d 85
Hendrikvg 21:394a7a1deb73 86 void ReadEncoder(){
Hendrikvg 22:6cc93216b323 87 theta_1 = ((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 88 }
Hendrikvg 16:40183eeadb6d 89
Hendrikvg 21:394a7a1deb73 90 void Motor_1() {
Hendrikvg 21:394a7a1deb73 91 ReadEncoder();
Hendrikvg 22:6cc93216b323 92 theta_reference_1 = 360*sin(0.1f*sinus_time.read()*2*3.14f);
Hendrikvg 22:6cc93216b323 93 CalculateDirectionMotor();
Hendrikvg 22:6cc93216b323 94 PWM_motor_1.write(fabs(Controller(CalculateError(theta_reference_1,theta_1))/360.0f));
Hendrikvg 21:394a7a1deb73 95 }
Hendrikvg 21:394a7a1deb73 96
Hendrikvg 21:394a7a1deb73 97 void go_next_1()
Hendrikvg 21:394a7a1deb73 98 {
Hendrikvg 21:394a7a1deb73 99 demo = !demo;
Hendrikvg 21:394a7a1deb73 100 }
Hendrikvg 21:394a7a1deb73 101
Hendrikvg 21:394a7a1deb73 102 void go_next_2()
Hendrikvg 21:394a7a1deb73 103 {
Hendrikvg 21:394a7a1deb73 104 emg = !emg;
Hendrikvg 21:394a7a1deb73 105 next = emg;
Hendrikvg 21:394a7a1deb73 106 }
Hendrikvg 21:394a7a1deb73 107
Hendrikvg 21:394a7a1deb73 108 void while_start()
Hendrikvg 21:394a7a1deb73 109 {
Hendrikvg 21:394a7a1deb73 110 // Do startup stuff
Hendrikvg 21:394a7a1deb73 111 CurrentState = motor_calibration;
Hendrikvg 21:394a7a1deb73 112 StateChanged = true;
Hendrikvg 21:394a7a1deb73 113 }
Hendrikvg 21:394a7a1deb73 114
Hendrikvg 21:394a7a1deb73 115 void while_motor_calibration()
Hendrikvg 21:394a7a1deb73 116 {
Hendrikvg 21:394a7a1deb73 117 // Do motor calibration stuff
Hendrikvg 21:394a7a1deb73 118 if (demo) // bool aanmaken voor demo (switch oid aanmaken)
Hendrikvg 21:394a7a1deb73 119 {
Hendrikvg 21:394a7a1deb73 120 CurrentState = demo_mode;
Hendrikvg 21:394a7a1deb73 121 StateChanged = true;
Hendrikvg 21:394a7a1deb73 122 }
Hendrikvg 21:394a7a1deb73 123 if (emg) // bool aanmaken voor EMG (switch oid aanmaken)
Hendrikvg 21:394a7a1deb73 124 {
Hendrikvg 21:394a7a1deb73 125 CurrentState = emg_calibration;
Hendrikvg 21:394a7a1deb73 126 StateChanged = true;
Hendrikvg 21:394a7a1deb73 127 }
Hendrikvg 21:394a7a1deb73 128 }
Hendrikvg 21:394a7a1deb73 129
Hendrikvg 21:394a7a1deb73 130 void while_demo_mode()
Hendrikvg 21:394a7a1deb73 131 {
Hendrikvg 21:394a7a1deb73 132 // Do demo mode stuff
Hendrikvg 21:394a7a1deb73 133 if (!demo) // bool aanmaken voor demo (switch oid)
Hendrikvg 21:394a7a1deb73 134 {
Hendrikvg 21:394a7a1deb73 135 emg = true;
Hendrikvg 21:394a7a1deb73 136 }
Hendrikvg 21:394a7a1deb73 137 if (emg) // bool aanmaken voor EMG (switch oid aanmaken)
Hendrikvg 21:394a7a1deb73 138 {
Hendrikvg 21:394a7a1deb73 139 CurrentState = emg_calibration;
Hendrikvg 21:394a7a1deb73 140 StateChanged = true;
Hendrikvg 21:394a7a1deb73 141 }
Hendrikvg 21:394a7a1deb73 142 }
Hendrikvg 21:394a7a1deb73 143
Hendrikvg 21:394a7a1deb73 144 void while_emg_calibration()
Hendrikvg 21:394a7a1deb73 145 {
Hendrikvg 21:394a7a1deb73 146 // Do emg calibration stuff
Hendrikvg 21:394a7a1deb73 147 if (!emg) // bool aanmaken voor EMG (switch)
Hendrikvg 21:394a7a1deb73 148 {
Hendrikvg 21:394a7a1deb73 149 CurrentState = vertical_movement;
Hendrikvg 21:394a7a1deb73 150 StateChanged = true;
Hendrikvg 21:394a7a1deb73 151 }
Hendrikvg 21:394a7a1deb73 152 }
Hendrikvg 21:394a7a1deb73 153
Hendrikvg 21:394a7a1deb73 154 void while_vertical_movement()
Hendrikvg 21:394a7a1deb73 155 {
Hendrikvg 21:394a7a1deb73 156 // Do vertical movement stuff
Hendrikvg 21:394a7a1deb73 157 if (next) // EMG gebaseerde threshold aanmaken
Hendrikvg 21:394a7a1deb73 158 {
Hendrikvg 21:394a7a1deb73 159 CurrentState = horizontal_movement;
Hendrikvg 21:394a7a1deb73 160 StateChanged = true;
Hendrikvg 21:394a7a1deb73 161 }
Hendrikvg 21:394a7a1deb73 162 }
Hendrikvg 17:cacf9e75eda7 163
Hendrikvg 21:394a7a1deb73 164 void while_horizontal_movement()
Hendrikvg 21:394a7a1deb73 165 {
Hendrikvg 21:394a7a1deb73 166 // Do horizontal movement stuff
Hendrikvg 21:394a7a1deb73 167 if (!next) // EMG gebaseerde threshold aanmaken
Hendrikvg 21:394a7a1deb73 168 {
Hendrikvg 21:394a7a1deb73 169 CurrentState = vertical_movement;
Hendrikvg 21:394a7a1deb73 170 StateChanged = true;
Hendrikvg 21:394a7a1deb73 171 }
Hendrikvg 21:394a7a1deb73 172 }
Hendrikvg 21:394a7a1deb73 173
Hendrikvg 21:394a7a1deb73 174 void StateTransition()
Hendrikvg 21:394a7a1deb73 175 {
Hendrikvg 21:394a7a1deb73 176 if (StateChanged)
Hendrikvg 21:394a7a1deb73 177 {
Hendrikvg 21:394a7a1deb73 178 if (CurrentState == start)
Hendrikvg 21:394a7a1deb73 179 {
Hendrikvg 21:394a7a1deb73 180 pc.printf("Initiating start.\n\r");
Hendrikvg 21:394a7a1deb73 181 }
Hendrikvg 21:394a7a1deb73 182 if (CurrentState == motor_calibration)
Hendrikvg 21:394a7a1deb73 183 {
Hendrikvg 21:394a7a1deb73 184 pc.printf("Initiating motor_calibration.\n\r");
Hendrikvg 21:394a7a1deb73 185 }
Hendrikvg 21:394a7a1deb73 186 if (CurrentState == demo_mode)
Hendrikvg 21:394a7a1deb73 187 {
Hendrikvg 21:394a7a1deb73 188 pc.printf("Initiating demo_mode.\n\r");
Hendrikvg 21:394a7a1deb73 189 }
Hendrikvg 21:394a7a1deb73 190 if (CurrentState == emg_calibration)
Hendrikvg 21:394a7a1deb73 191 {
Hendrikvg 21:394a7a1deb73 192 pc.printf("Initiating emg_calibration.\n\r");
Hendrikvg 21:394a7a1deb73 193 }
Hendrikvg 21:394a7a1deb73 194 if (CurrentState == vertical_movement)
Hendrikvg 21:394a7a1deb73 195 {
Hendrikvg 21:394a7a1deb73 196 pc.printf("Initiating vertical_movement.\n\r");
Hendrikvg 21:394a7a1deb73 197 }
Hendrikvg 21:394a7a1deb73 198 if (CurrentState == horizontal_movement)
Hendrikvg 21:394a7a1deb73 199 {
Hendrikvg 21:394a7a1deb73 200 pc.printf("Initiating horizontal_movement.\n\r");
Hendrikvg 21:394a7a1deb73 201 }
Hendrikvg 21:394a7a1deb73 202 StateChanged = false;
Hendrikvg 21:394a7a1deb73 203 }
Hendrikvg 21:394a7a1deb73 204 }
Hendrikvg 21:394a7a1deb73 205
Hendrikvg 21:394a7a1deb73 206 void StateMachine()
Hendrikvg 21:394a7a1deb73 207 {
Hendrikvg 21:394a7a1deb73 208 switch(CurrentState)
Hendrikvg 21:394a7a1deb73 209 {
Hendrikvg 21:394a7a1deb73 210 case start:
Hendrikvg 21:394a7a1deb73 211 StateTransition();
Hendrikvg 21:394a7a1deb73 212 while_start();
Hendrikvg 21:394a7a1deb73 213 break;
Hendrikvg 21:394a7a1deb73 214 case motor_calibration:
Hendrikvg 21:394a7a1deb73 215 StateTransition();
Hendrikvg 21:394a7a1deb73 216 while_motor_calibration();
Hendrikvg 21:394a7a1deb73 217 break;
Hendrikvg 21:394a7a1deb73 218 case demo_mode:
Hendrikvg 21:394a7a1deb73 219 StateTransition();
Hendrikvg 21:394a7a1deb73 220 while_demo_mode();
Hendrikvg 21:394a7a1deb73 221 break;
Hendrikvg 21:394a7a1deb73 222 case emg_calibration:
Hendrikvg 21:394a7a1deb73 223 StateTransition();
Hendrikvg 21:394a7a1deb73 224 while_emg_calibration();
Hendrikvg 21:394a7a1deb73 225 break;
Hendrikvg 21:394a7a1deb73 226 case vertical_movement:
Hendrikvg 21:394a7a1deb73 227 StateTransition();
Hendrikvg 21:394a7a1deb73 228 while_vertical_movement();
Hendrikvg 21:394a7a1deb73 229 break;
Hendrikvg 21:394a7a1deb73 230 case horizontal_movement:
Hendrikvg 21:394a7a1deb73 231 StateTransition();
Hendrikvg 21:394a7a1deb73 232 while_horizontal_movement();
Hendrikvg 21:394a7a1deb73 233 break;
Hendrikvg 21:394a7a1deb73 234 default:
Hendrikvg 21:394a7a1deb73 235 pc.printf("Unknown or unimplemented state reached!\n\r");
Hendrikvg 21:394a7a1deb73 236 }
Hendrikvg 21:394a7a1deb73 237 }
Hendrikvg 21:394a7a1deb73 238
Hendrikvg 15:80b3ac2b8448 239 // main
Hendrikvg 21:394a7a1deb73 240 int main(){
RobertoO 0:67c50348f842 241 pc.baud(115200);
Hendrikvg 17:cacf9e75eda7 242 pc.printf("Hello World!\n\r");
Hendrikvg 21:394a7a1deb73 243 button_1.fall(go_next_1);
Hendrikvg 21:394a7a1deb73 244 button_2.fall(go_next_2);
Hendrikvg 21:394a7a1deb73 245 sinus_time.start();
Hendrikvg 21:394a7a1deb73 246 PWM_motor_1.period_ms(10);
Hendrikvg 21:394a7a1deb73 247 ControlMotor1.attach(&Motor_1, Ts);
Hendrikvg 21:394a7a1deb73 248 RW_scope.attach(&WriteScope, 0.1);
Hendrikvg 21:394a7a1deb73 249 //TickerStateMachine.attach(StateMachine,1.00f);
Hendrikvg 21:394a7a1deb73 250 while(true) {
Hendrikvg 21:394a7a1deb73 251 StateMachine();
Hendrikvg 21:394a7a1deb73 252 }
Hendrikvg 2:d9b0ebf3fcca 253 }