Biorobotics 2019 groep 24 / Mbed 2 deprecated Final_code

Calibratie motor, demo, (EMG calibratie en movement werkt niet)

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL Encoder FastPWM

main.cpp@4:7d0df890e801, 2019-10-30 (annotated)

Committer:
S1933191
Date:
Wed Oct 30 16:07:58 2019 +0000
Revision:
4:7d0df890e801
Parent:
3:16df793da2be
Child:
5:810892d669f9
juiste QEI library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
S1933191 2:5093b66c9b26 1 #include <math.h>
RobertoO 0:67c50348f842 2 #include "mbed.h"
RobertoO 1:b862262a9d14 3 #include "MODSERIAL.h"
S1933191 2:5093b66c9b26 4 #include "QEI.h"
S1933191 2:5093b66c9b26 5 #include "FastPWM.h"
S1933191 2:5093b66c9b26 6 #include "encoder.h"
S1933191 2:5093b66c9b26 7
S1933191 2:5093b66c9b26 8 const double PI = 3.1415926535897;
S1933191 2:5093b66c9b26 9
S1933191 3:16df793da2be 10 InterruptIn but1(D1);
S1933191 3:16df793da2be 11 InterruptIn but2(D0);
S1933191 4:7d0df890e801 12 DigitalIn butsw2(SW3);
S1933191 4:7d0df890e801 13 DigitalIn butsw3(SW2);
S1933191 2:5093b66c9b26 14 AnalogIn potMeter1(A2);
S1933191 2:5093b66c9b26 15 AnalogIn potMeter2(A1);
S1933191 2:5093b66c9b26 16
S1933191 2:5093b66c9b26 17 DigitalOut motor1_direction(D4); //value 0=CCW, 1=CW
S1933191 2:5093b66c9b26 18 FastPWM motor1_pwm(D5); //Biorobotics Motor 1 PWM controle van de snelheid
S1933191 2:5093b66c9b26 19 DigitalOut motor2_direction(D7); //value 0=CCW, 1=CW
S1933191 2:5093b66c9b26 20 FastPWM motor2_pwm(D6);
S1933191 2:5093b66c9b26 21
S1933191 2:5093b66c9b26 22 Ticker loop_ticker;
S1933191 2:5093b66c9b26 23 // SERIAL COMMUNICATION WITH PC.................................................
S1933191 2:5093b66c9b26 24 Serial pc(USBTX, USBRX);
S1933191 2:5093b66c9b26 25
S1933191 3:16df793da2be 26 enum States {s_idle, s_calibratie_encoder, s_demo};
S1933191 2:5093b66c9b26 27 States state;
S1933191 2:5093b66c9b26 28
S1933191 2:5093b66c9b26 29 // ENCODER .....................................................................
S1933191 4:7d0df890e801 30 QEI enc1 (D13, D12, NC, 4200); //encoder 1 gebruiken
S1933191 4:7d0df890e801 31 QEI enc2 (D9, D8, NC, 4200); //encoder 2 gebruiken
S1933191 2:5093b66c9b26 32
S1933191 2:5093b66c9b26 33 float dt = 0.001;
S1933191 2:5093b66c9b26 34 double e_int = 0;
S1933191 2:5093b66c9b26 35 double e_int2 = 0;
S1933191 2:5093b66c9b26 36 float theta1;
S1933191 2:5093b66c9b26 37 float theta2;
S1933191 2:5093b66c9b26 38 int enc1_zero = 0;//the zero position of the encoders, to be determined from the
S1933191 2:5093b66c9b26 39 int enc2_zero = 0;//encoder calibration
S1933191 2:5093b66c9b26 40 int enc1_value;
S1933191 2:5093b66c9b26 41 int enc2_value;
S1933191 3:16df793da2be 42 volatile double Huidigepositie1;
S1933191 3:16df793da2be 43 volatile double Huidigepositie2;
S1933191 3:16df793da2be 44 volatile double motorValue1;
S1933191 3:16df793da2be 45 volatile double motorValue2;
S1933191 3:16df793da2be 46 volatile double refP;
S1933191 3:16df793da2be 47 volatile double refP2;
S1933191 3:16df793da2be 48 volatile double error1;
S1933191 3:16df793da2be 49 volatile double error2;
S1933191 2:5093b66c9b26 50 bool state_changed = false;
S1933191 3:16df793da2be 51 volatile bool A=true;
S1933191 3:16df793da2be 52 volatile bool B=true;
S1933191 2:5093b66c9b26 53 volatile bool but1_pressed = false;
S1933191 2:5093b66c9b26 54 volatile bool but2_pressed = false;
S1933191 3:16df793da2be 55 volatile bool butsw2_pressed = false;
S1933191 3:16df793da2be 56 volatile bool butsw3_pressed = false;
S1933191 2:5093b66c9b26 57 volatile bool failure_occurred = false;
S1933191 2:5093b66c9b26 58
S1933191 3:16df793da2be 59 const bool clockwise = true;
S1933191 3:16df793da2be 60 volatile bool direction1 = clockwise;
S1933191 3:16df793da2be 61 volatile bool direction2 = clockwise;
S1933191 3:16df793da2be 62
S1933191 3:16df793da2be 63 void rest()
S1933191 2:5093b66c9b26 64 //Idle state. Used in the beginning, before the calibration states.
S1933191 2:5093b66c9b26 65 {
S1933191 2:5093b66c9b26 66 if (but1_pressed) {
S1933191 2:5093b66c9b26 67 state_changed = true;
S1933191 3:16df793da2be 68 state = s_calibratie_encoder;
S1933191 2:5093b66c9b26 69 but1_pressed = false;
S1933191 2:5093b66c9b26 70 }
S1933191 2:5093b66c9b26 71 }
RobertoO 0:67c50348f842 72
S1933191 3:16df793da2be 73 void calibratie_encoder()
S1933191 2:5093b66c9b26 74 {
S1933191 2:5093b66c9b26 75 if (state_changed) {
S1933191 2:5093b66c9b26 76 pc.printf("Started encoder calibration\r\n");
S1933191 2:5093b66c9b26 77 state_changed = false;
S1933191 2:5093b66c9b26 78 }
S1933191 2:5093b66c9b26 79 if (but1_pressed) {
S1933191 2:5093b66c9b26 80 pc.printf("Encoder has been calibrated \r\n");
S1933191 4:7d0df890e801 81 enc1_value = enc1.getPulses();
S1933191 4:7d0df890e801 82 enc2_value = enc2.getPulses();
S1933191 3:16df793da2be 83 //enc1_value -= enc1_zero;
S1933191 3:16df793da2be 84 //enc2_value -= enc2_zero;
S1933191 4:7d0df890e801 85 theta1 = (float)(enc1_value)/(float)(4200)*2*PI;
S1933191 4:7d0df890e801 86 theta2 = (float)(enc2_value)/(float)(4200)*2*PI;
S1933191 2:5093b66c9b26 87 enc1_zero = enc1_value;
S1933191 2:5093b66c9b26 88 enc2_zero = enc2_value;
S1933191 2:5093b66c9b26 89 but1_pressed = false;
S1933191 2:5093b66c9b26 90 state_changed = true;
S1933191 2:5093b66c9b26 91 state = s_demo;
S1933191 2:5093b66c9b26 92 pc.printf("enc01: %i\r\n", enc1_zero);
S1933191 2:5093b66c9b26 93 pc.printf("enc1value: %i\r\n", enc1_value);
S1933191 2:5093b66c9b26 94 pc.printf("enc02: %i\r\n",enc2_zero);
S1933191 2:5093b66c9b26 95 pc.printf("enc2value: %i\r\n", enc2_value);
S1933191 2:5093b66c9b26 96 pc.printf("hoek 1: %f\r\n",theta1);
S1933191 2:5093b66c9b26 97 pc.printf("hoek 2: %f\r\n",theta2);
S1933191 3:16df793da2be 98 }
S1933191 2:5093b66c9b26 99
S1933191 3:16df793da2be 100 if (but2_pressed) {
S1933191 3:16df793da2be 101 state_changed = true;
S1933191 3:16df793da2be 102 state = s_idle;
S1933191 3:16df793da2be 103 but2_pressed = false;
S1933191 3:16df793da2be 104 pc.printf("FAILURE!\r\n");
S1933191 3:16df793da2be 105 state_changed = false;
S1933191 2:5093b66c9b26 106 }
S1933191 2:5093b66c9b26 107 }
S1933191 3:16df793da2be 108
S1933191 2:5093b66c9b26 109 double GetReferencePosition()
S1933191 2:5093b66c9b26 110 {
S1933191 3:16df793da2be 111 double Potmeterwaarde = potMeter2.read(); //naam moet universeel worden
S1933191 4:7d0df890e801 112 int maxwaarde = 400; //
S1933191 2:5093b66c9b26 113 double refP = Potmeterwaarde*maxwaarde;
S1933191 4:7d0df890e801 114 return refP; //
S1933191 2:5093b66c9b26 115 }
S1933191 2:5093b66c9b26 116
S1933191 2:5093b66c9b26 117 double GetReferencePosition2()
S1933191 2:5093b66c9b26 118 {
S1933191 3:16df793da2be 119 double potmeterwaarde2 = potMeter1.read();
S1933191 4:7d0df890e801 120 int maxwaarde2 = 400; //
S1933191 2:5093b66c9b26 121 double refP2 = potmeterwaarde2*maxwaarde2;
S1933191 4:7d0df890e801 122 return refP2; //
S1933191 2:5093b66c9b26 123 }
S1933191 2:5093b66c9b26 124
S1933191 2:5093b66c9b26 125 double FeedBackControl(double error, double &e_int) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
S1933191 2:5093b66c9b26 126 {
S1933191 2:5093b66c9b26 127 double kp = 0.0015; // kind of scaled.
S1933191 3:16df793da2be 128 double Proportional= kp*error1;
S1933191 2:5093b66c9b26 129
S1933191 2:5093b66c9b26 130 double ki = 0.0001; // kind of scaled.
S1933191 3:16df793da2be 131 e_int = e_int+dt*error1;
S1933191 2:5093b66c9b26 132 double Integrator = ki*e_int;
S1933191 2:5093b66c9b26 133
S1933191 3:16df793da2be 134 motorValue1 = Proportional + Integrator ;
S1933191 3:16df793da2be 135 return motorValue1;
S1933191 2:5093b66c9b26 136 }
S1933191 2:5093b66c9b26 137
S1933191 2:5093b66c9b26 138 double FeedBackControl2(double error2, double &e_int2) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
S1933191 2:5093b66c9b26 139 {
S1933191 2:5093b66c9b26 140 double kp2 = 0.002; // kind of scaled.
S1933191 2:5093b66c9b26 141 double Proportional2= kp2*error2;
S1933191 2:5093b66c9b26 142
S1933191 2:5093b66c9b26 143 double ki2 = 0.00005; // kind of scaled.
S1933191 2:5093b66c9b26 144 e_int2 = e_int2+dt*error2;
S1933191 2:5093b66c9b26 145 double Integrator2 = ki2*e_int2;
S1933191 2:5093b66c9b26 146
S1933191 2:5093b66c9b26 147 double motorValue2 = Proportional2 + Integrator2 ;
S1933191 2:5093b66c9b26 148 return motorValue2;
S1933191 3:16df793da2be 149 }
S1933191 3:16df793da2be 150
S1933191 3:16df793da2be 151 void SetMotor1(float motorValue1) {
S1933191 3:16df793da2be 152 // Given motorValue1<=1, writes the velocity to the pwm control.
S1933191 3:16df793da2be 153 // MotorValues outside range are truncated to within range.
S1933191 3:16df793da2be 154 motor1_pwm.write(fabs(motorValue1) > 1 ? 1 : fabs(motorValue1));
S1933191 2:5093b66c9b26 155 }
S1933191 3:16df793da2be 156
S1933191 3:16df793da2be 157 void SetMotor2(float motorValue2) {
S1933191 3:16df793da2be 158 // Given motorValue2<=1, writes the velocity to the pwm control.
S1933191 3:16df793da2be 159 // MotorValues outside range are truncated to within range.
S1933191 3:16df793da2be 160 motor2_pwm.write(fabs(motorValue2) > 1 ? 1 : fabs(motorValue2));
S1933191 3:16df793da2be 161 }
S1933191 2:5093b66c9b26 162
S1933191 2:5093b66c9b26 163 void MeasureAndControl(void)
S1933191 2:5093b66c9b26 164 {
S1933191 2:5093b66c9b26 165 //SetpointRobot();
S1933191 2:5093b66c9b26 166 // RKI aanroepen
S1933191 2:5093b66c9b26 167 //RKI();
S1933191 2:5093b66c9b26 168 // hier the control of the 1st control system
S1933191 3:16df793da2be 169 refP = GetReferencePosition(); //moet nog met RKI
S1933191 4:7d0df890e801 170 Huidigepositie1 = enc1.getPulses();
S1933191 3:16df793da2be 171 error1 = (refP - Huidigepositie1);// make an error
S1933191 3:16df793da2be 172 motorValue1 = FeedBackControl(error1, e_int);
S1933191 3:16df793da2be 173 SetMotor1(motorValue1);
S1933191 2:5093b66c9b26 174 // hier the control of the 2nd control system
S1933191 3:16df793da2be 175 refP2 = GetReferencePosition2();
S1933191 4:7d0df890e801 176 Huidigepositie2 = enc2.getPulses();
S1933191 3:16df793da2be 177 error2 = (refP2 - Huidigepositie2);// make an error
S1933191 3:16df793da2be 178 motorValue2 = FeedBackControl2(error2, e_int2);
S1933191 2:5093b66c9b26 179 SetMotor2(motorValue2);
S1933191 3:16df793da2be 180 pc.printf("refP = %f, huidigepos = %f, motorvalue = %f, refP2 = %f, huidigepos2 = %f, motorvalue2 = %f \r\n", refP, Huidigepositie1, motorValue1, refP2, Huidigepositie2, Huidigepositie2);
S1933191 3:16df793da2be 181
S1933191 2:5093b66c9b26 182 }
S1933191 3:16df793da2be 183
S1933191 3:16df793da2be 184 void direction()
S1933191 3:16df793da2be 185 {
S1933191 3:16df793da2be 186 if (butsw2==0) {
S1933191 3:16df793da2be 187 if (A==true){// zodat het knopje 1 x wordt afgelezen
S1933191 3:16df793da2be 188 // reverses the direction
S1933191 3:16df793da2be 189 motor1_direction.write(direction1 = !direction1);
S1933191 3:16df793da2be 190 pc.printf("direction: %s\r\n\n", direction1 ? "clockwise" : "counter clockwise");
S1933191 3:16df793da2be 191 A=false;
S1933191 3:16df793da2be 192 }
S1933191 3:16df793da2be 193 }
S1933191 3:16df793da2be 194 else{
S1933191 3:16df793da2be 195 A=true;
S1933191 3:16df793da2be 196 }
S1933191 2:5093b66c9b26 197
S1933191 3:16df793da2be 198 if (butsw3==0){
S1933191 3:16df793da2be 199 if (B==true){
S1933191 3:16df793da2be 200 motor2_direction.write(direction2 = !direction2);
S1933191 3:16df793da2be 201 pc.printf("direction: %s\r\n\n", direction2 ? "clockwise" : "counter clockwise");
S1933191 3:16df793da2be 202 B=false;
S1933191 3:16df793da2be 203 }
S1933191 3:16df793da2be 204 }
S1933191 3:16df793da2be 205 else{
S1933191 3:16df793da2be 206 B=true;
S1933191 3:16df793da2be 207 }
S1933191 3:16df793da2be 208 }
S1933191 3:16df793da2be 209
S1933191 3:16df793da2be 210 /*void measure_signals()
S1933191 2:5093b66c9b26 211 {
S1933191 4:7d0df890e801 212 enc1_value = enc1.getPulses();
S1933191 4:7d0df890e801 213 enc2_value = enc2.getPulses();
S1933191 4:7d0df890e801 214 enc1_value -= enc1_zero;
S1933191 3:16df793da2be 215 //enc2_value -= enc2_zero;
S1933191 2:5093b66c9b26 216 theta1 = (float)(enc1_value)/(float)(8400)*2*PI;
S1933191 2:5093b66c9b26 217 theta2 = (float)(enc2_value)/(float)(8400)*2*PI;
S1933191 2:5093b66c9b26 218 }
S1933191 3:16df793da2be 219 */
S1933191 2:5093b66c9b26 220 void state_machine()
S1933191 2:5093b66c9b26 221 {
S1933191 2:5093b66c9b26 222 //run current state
S1933191 2:5093b66c9b26 223 switch (state) {
S1933191 2:5093b66c9b26 224 case s_idle:
S1933191 3:16df793da2be 225 rest();
S1933191 2:5093b66c9b26 226 break;
S1933191 3:16df793da2be 227 case s_calibratie_encoder:
S1933191 3:16df793da2be 228 calibratie_encoder();
S1933191 2:5093b66c9b26 229 break;
S1933191 2:5093b66c9b26 230 case s_demo:
S1933191 3:16df793da2be 231 MeasureAndControl();
S1933191 3:16df793da2be 232 direction();
S1933191 2:5093b66c9b26 233 break;
S1933191 2:5093b66c9b26 234
S1933191 2:5093b66c9b26 235 }
S1933191 2:5093b66c9b26 236 }
RobertoO 0:67c50348f842 237
S1933191 2:5093b66c9b26 238 void but1_interrupt()
S1933191 2:5093b66c9b26 239 {
S1933191 2:5093b66c9b26 240 if(but2.read()) {//both buttons are pressed
S1933191 2:5093b66c9b26 241 failure_occurred = true;
S1933191 2:5093b66c9b26 242 }
S1933191 2:5093b66c9b26 243 but1_pressed = true;
S1933191 3:16df793da2be 244 pc.printf("Button 1 pressed \n\r");
S1933191 2:5093b66c9b26 245 }
S1933191 2:5093b66c9b26 246
S1933191 2:5093b66c9b26 247 void but2_interrupt()
S1933191 2:5093b66c9b26 248 {
S1933191 2:5093b66c9b26 249 if(but1.read()) {//both buttons are pressed
S1933191 2:5093b66c9b26 250 failure_occurred = true;
S1933191 2:5093b66c9b26 251 }
S1933191 2:5093b66c9b26 252 but2_pressed = true;
S1933191 2:5093b66c9b26 253 pc.printf("Button 2 pressed \n\r");
S1933191 2:5093b66c9b26 254 }
S1933191 3:16df793da2be 255
S1933191 2:5093b66c9b26 256 void main_loop()
S1933191 2:5093b66c9b26 257 {
S1933191 3:16df793da2be 258 //measure_signals();
S1933191 2:5093b66c9b26 259 state_machine();
S1933191 2:5093b66c9b26 260 }
RobertoO 0:67c50348f842 261
RobertoO 0:67c50348f842 262 int main()
RobertoO 0:67c50348f842 263 {
RobertoO 0:67c50348f842 264 pc.baud(115200);
S1933191 2:5093b66c9b26 265 pc.printf("Executing main()... \r\n");
S1933191 2:5093b66c9b26 266 state = s_idle;
S1933191 2:5093b66c9b26 267
RobertoO 0:67c50348f842 268
S1933191 2:5093b66c9b26 269 but1.fall(&but1_interrupt);
S1933191 2:5093b66c9b26 270 but2.fall(&but2_interrupt);
S1933191 2:5093b66c9b26 271 loop_ticker.attach(&main_loop, dt);
S1933191 2:5093b66c9b26 272 pc.printf("main_loop is running\n\r");
S1933191 2:5093b66c9b26 273
S1933191 2:5093b66c9b26 274 }
S1933191 2:5093b66c9b26 275