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@2:5093b66c9b26, 2019-10-29 (annotated)

Committer:
S1933191
Date:
Tue Oct 29 15:48:51 2019 +0000
Revision:
2:5093b66c9b26
Parent:
1:b862262a9d14
Child:
3:16df793da2be
Motorcalibratie + demo(alleen nog snelheid aan/uit) direction buttons moet nog;

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 2:5093b66c9b26 10 InterruptIn but1(D2);
S1933191 2:5093b66c9b26 11 InterruptIn but2(D3);
S1933191 2:5093b66c9b26 12 AnalogIn potMeter1(A2);
S1933191 2:5093b66c9b26 13 AnalogIn potMeter2(A1);
S1933191 2:5093b66c9b26 14
S1933191 2:5093b66c9b26 15 DigitalOut motor1_direction(D4); //value 0=CCW, 1=CW
S1933191 2:5093b66c9b26 16 FastPWM motor1_pwm(D5); //Biorobotics Motor 1 PWM controle van de snelheid
S1933191 2:5093b66c9b26 17 DigitalOut motor2_direction(D7); //value 0=CCW, 1=CW
S1933191 2:5093b66c9b26 18 FastPWM motor2_pwm(D6);
S1933191 2:5093b66c9b26 19
S1933191 2:5093b66c9b26 20 Ticker loop_ticker;
S1933191 2:5093b66c9b26 21 // SERIAL COMMUNICATION WITH PC.................................................
S1933191 2:5093b66c9b26 22 Serial pc(USBTX, USBRX);
S1933191 2:5093b66c9b26 23
S1933191 2:5093b66c9b26 24 enum States {s_idle, s_cali_enc, s_demo};
S1933191 2:5093b66c9b26 25 States state;
S1933191 2:5093b66c9b26 26
S1933191 2:5093b66c9b26 27 // ENCODER .....................................................................
S1933191 2:5093b66c9b26 28 Encoder enc1 (D13, D12, true); //encoder 1 gebruiken
S1933191 2:5093b66c9b26 29 Encoder enc2 (D9, D8, true); //encoder 2 gebruiken
S1933191 2:5093b66c9b26 30
S1933191 2:5093b66c9b26 31 float dt = 0.001;
S1933191 2:5093b66c9b26 32 double e_int = 0;
S1933191 2:5093b66c9b26 33 double e_int2 = 0;
S1933191 2:5093b66c9b26 34 float theta1;
S1933191 2:5093b66c9b26 35 float theta2;
S1933191 2:5093b66c9b26 36 int enc1_zero = 0;//the zero position of the encoders, to be determined from the
S1933191 2:5093b66c9b26 37 int enc2_zero = 0;//encoder calibration
S1933191 2:5093b66c9b26 38 int enc1_value;
S1933191 2:5093b66c9b26 39 int enc2_value;
S1933191 2:5093b66c9b26 40 bool state_changed = false;
S1933191 2:5093b66c9b26 41 volatile bool but1_pressed = false;
S1933191 2:5093b66c9b26 42 volatile bool but2_pressed = false;
S1933191 2:5093b66c9b26 43 volatile bool failure_occurred = false;
S1933191 2:5093b66c9b26 44
S1933191 2:5093b66c9b26 45 void do_nothing()
S1933191 2:5093b66c9b26 46 //Idle state. Used in the beginning, before the calibration states.
S1933191 2:5093b66c9b26 47 {
S1933191 2:5093b66c9b26 48 if (but1_pressed) {
S1933191 2:5093b66c9b26 49 state_changed = true;
S1933191 2:5093b66c9b26 50 state = s_cali_enc;
S1933191 2:5093b66c9b26 51 but1_pressed = false;
S1933191 2:5093b66c9b26 52 }
S1933191 2:5093b66c9b26 53 }
RobertoO 0:67c50348f842 54
S1933191 2:5093b66c9b26 55 void cali_enc()
S1933191 2:5093b66c9b26 56 {
S1933191 2:5093b66c9b26 57 if (state_changed) {
S1933191 2:5093b66c9b26 58 pc.printf("Started encoder calibration\r\n");
S1933191 2:5093b66c9b26 59 state_changed = false;
S1933191 2:5093b66c9b26 60 }
S1933191 2:5093b66c9b26 61 if (but1_pressed) {
S1933191 2:5093b66c9b26 62 pc.printf("Encoder has been calibrated \r\n");
S1933191 2:5093b66c9b26 63 enc1_zero = enc1_value;
S1933191 2:5093b66c9b26 64 enc2_zero = enc2_value;
S1933191 2:5093b66c9b26 65 but1_pressed = false;
S1933191 2:5093b66c9b26 66 state_changed = true;
S1933191 2:5093b66c9b26 67 state = s_demo;
S1933191 2:5093b66c9b26 68 pc.printf("enc01: %i\r\n", enc1_zero);
S1933191 2:5093b66c9b26 69 pc.printf("enc1value: %i\r\n", enc1_value);
S1933191 2:5093b66c9b26 70 pc.printf("enc02: %i\r\n",enc2_zero);
S1933191 2:5093b66c9b26 71 pc.printf("enc2value: %i\r\n", enc2_value);
S1933191 2:5093b66c9b26 72 pc.printf("hoek 1: %f\r\n",theta1);
S1933191 2:5093b66c9b26 73 pc.printf("hoek 2: %f\r\n",theta2);
S1933191 2:5093b66c9b26 74
S1933191 2:5093b66c9b26 75
S1933191 2:5093b66c9b26 76 }
S1933191 2:5093b66c9b26 77 }
S1933191 2:5093b66c9b26 78
S1933191 2:5093b66c9b26 79 double GetReferencePosition()
S1933191 2:5093b66c9b26 80 {
S1933191 2:5093b66c9b26 81 double Potmeterwaarde = potMeter1.read(); //naam moet universeel worden
S1933191 2:5093b66c9b26 82 int maxwaarde = 200; // = 64x64
S1933191 2:5093b66c9b26 83 double refP = Potmeterwaarde*maxwaarde;
S1933191 2:5093b66c9b26 84 return refP; // value between 0 and 4096
S1933191 2:5093b66c9b26 85 }
S1933191 2:5093b66c9b26 86
S1933191 2:5093b66c9b26 87 double GetReferencePosition2()
S1933191 2:5093b66c9b26 88 {
S1933191 2:5093b66c9b26 89 double potmeterwaarde2 = potMeter2.read();
S1933191 2:5093b66c9b26 90 int maxwaarde2 = 200; // = 64x64
S1933191 2:5093b66c9b26 91 double refP2 = potmeterwaarde2*maxwaarde2;
S1933191 2:5093b66c9b26 92 return refP2; // value between 0 and 4096
S1933191 2:5093b66c9b26 93 }
S1933191 2:5093b66c9b26 94
S1933191 2:5093b66c9b26 95 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 96 {
S1933191 2:5093b66c9b26 97 double kp = 0.0015; // kind of scaled.
S1933191 2:5093b66c9b26 98 double Proportional= kp*error;
S1933191 2:5093b66c9b26 99
S1933191 2:5093b66c9b26 100 double ki = 0.0001; // kind of scaled.
S1933191 2:5093b66c9b26 101 e_int = e_int+dt*error;
S1933191 2:5093b66c9b26 102 double Integrator = ki*e_int;
S1933191 2:5093b66c9b26 103
S1933191 2:5093b66c9b26 104
S1933191 2:5093b66c9b26 105 double motorValue = Proportional + Integrator ;
S1933191 2:5093b66c9b26 106 return motorValue;
S1933191 2:5093b66c9b26 107 }
S1933191 2:5093b66c9b26 108
S1933191 2:5093b66c9b26 109 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 110 {
S1933191 2:5093b66c9b26 111 double kp2 = 0.002; // kind of scaled.
S1933191 2:5093b66c9b26 112 double Proportional2= kp2*error2;
S1933191 2:5093b66c9b26 113
S1933191 2:5093b66c9b26 114 double ki2 = 0.00005; // kind of scaled.
S1933191 2:5093b66c9b26 115 e_int2 = e_int2+dt*error2;
S1933191 2:5093b66c9b26 116 double Integrator2 = ki2*e_int2;
S1933191 2:5093b66c9b26 117
S1933191 2:5093b66c9b26 118
S1933191 2:5093b66c9b26 119 double motorValue2 = Proportional2 + Integrator2 ;
S1933191 2:5093b66c9b26 120 return motorValue2;
S1933191 2:5093b66c9b26 121
S1933191 2:5093b66c9b26 122 }
S1933191 2:5093b66c9b26 123
S1933191 2:5093b66c9b26 124 void SetMotor1(double motorValue)
S1933191 2:5093b66c9b26 125 {
S1933191 2:5093b66c9b26 126 if (motorValue >= 0) {
S1933191 2:5093b66c9b26 127 motor1_direction = 0;
S1933191 2:5093b66c9b26 128 }
S1933191 2:5093b66c9b26 129 else {
S1933191 2:5093b66c9b26 130 motor1_direction = 1;
S1933191 2:5093b66c9b26 131 }
S1933191 2:5093b66c9b26 132
S1933191 2:5093b66c9b26 133 if (fabs(motorValue) > 1) {
S1933191 2:5093b66c9b26 134 motor1_pwm = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
S1933191 2:5093b66c9b26 135 }
S1933191 2:5093b66c9b26 136 else {
S1933191 2:5093b66c9b26 137 motor1_pwm = fabs(motorValue); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
S1933191 2:5093b66c9b26 138 }
S1933191 2:5093b66c9b26 139 }
S1933191 2:5093b66c9b26 140
S1933191 2:5093b66c9b26 141 void SetMotor2(double motorValue2)
S1933191 2:5093b66c9b26 142 {
S1933191 2:5093b66c9b26 143 if (motorValue2 >= 0) {
S1933191 2:5093b66c9b26 144 motor2_direction = 1;
S1933191 2:5093b66c9b26 145 }
S1933191 2:5093b66c9b26 146 else {
S1933191 2:5093b66c9b26 147 motor2_direction = 0;
S1933191 2:5093b66c9b26 148 }
S1933191 2:5093b66c9b26 149
S1933191 2:5093b66c9b26 150 if (fabs(motorValue2) > 1) {
S1933191 2:5093b66c9b26 151 motor2_pwm = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
S1933191 2:5093b66c9b26 152 }
S1933191 2:5093b66c9b26 153 else {
S1933191 2:5093b66c9b26 154 motor2_pwm = fabs(motorValue2); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
S1933191 2:5093b66c9b26 155 }
S1933191 2:5093b66c9b26 156 }
S1933191 2:5093b66c9b26 157
S1933191 2:5093b66c9b26 158 void MeasureAndControl(void)
S1933191 2:5093b66c9b26 159 {
S1933191 2:5093b66c9b26 160 //SetpointRobot();
S1933191 2:5093b66c9b26 161 // RKI aanroepen
S1933191 2:5093b66c9b26 162 //RKI();
S1933191 2:5093b66c9b26 163 // hier the control of the 1st control system
S1933191 2:5093b66c9b26 164 double refP = GetReferencePosition(); //KOMT UIT RKI
S1933191 2:5093b66c9b26 165 double Huidigepositie = enc1.getPosition();
S1933191 2:5093b66c9b26 166 double error = (refP - Huidigepositie);// make an error
S1933191 2:5093b66c9b26 167 double motorValue = FeedBackControl(error, e_int);
S1933191 2:5093b66c9b26 168 SetMotor1(motorValue);
S1933191 2:5093b66c9b26 169 // hier the control of the 2nd control system
S1933191 2:5093b66c9b26 170 double refP2 = GetReferencePosition2();
S1933191 2:5093b66c9b26 171 double Huidigepositie2 = enc2.getPosition();
S1933191 2:5093b66c9b26 172 double error2 = (refP2 - Huidigepositie2);// make an error
S1933191 2:5093b66c9b26 173 double motorValue2 = FeedBackControl2(error2, e_int2);
S1933191 2:5093b66c9b26 174 SetMotor2(motorValue2);
S1933191 2:5093b66c9b26 175 pc.printf("refP = %f, huidigepos = %f, motorvalue = %f, refP2 = %f, huidigepos2 = %f, motorvalue2 = %f \r\n", refP, Huidigepositie, motorValue, refP2, Huidigepositie2, Huidigepositie2);
S1933191 2:5093b66c9b26 176 }
S1933191 2:5093b66c9b26 177
S1933191 2:5093b66c9b26 178 void measure_signals()
S1933191 2:5093b66c9b26 179 {
S1933191 2:5093b66c9b26 180 enc1_value = enc1.getPosition();
S1933191 2:5093b66c9b26 181 enc2_value = enc2.getPosition();
S1933191 2:5093b66c9b26 182 enc1_value -= enc1_zero;
S1933191 2:5093b66c9b26 183 enc2_value -= enc2_zero;
S1933191 2:5093b66c9b26 184 theta1 = (float)(enc1_value)/(float)(8400)*2*PI;
S1933191 2:5093b66c9b26 185 theta2 = (float)(enc2_value)/(float)(8400)*2*PI;
S1933191 2:5093b66c9b26 186
S1933191 2:5093b66c9b26 187 }
S1933191 2:5093b66c9b26 188
S1933191 2:5093b66c9b26 189 void state_machine()
S1933191 2:5093b66c9b26 190 {
S1933191 2:5093b66c9b26 191 //run current state
S1933191 2:5093b66c9b26 192 switch (state) {
S1933191 2:5093b66c9b26 193 case s_idle:
S1933191 2:5093b66c9b26 194 do_nothing();
S1933191 2:5093b66c9b26 195 break;
S1933191 2:5093b66c9b26 196 case s_cali_enc:
S1933191 2:5093b66c9b26 197 cali_enc();
S1933191 2:5093b66c9b26 198 break;
S1933191 2:5093b66c9b26 199 case s_demo:
S1933191 2:5093b66c9b26 200 MeasureAndControl();
S1933191 2:5093b66c9b26 201 break;
S1933191 2:5093b66c9b26 202
S1933191 2:5093b66c9b26 203 }
S1933191 2:5093b66c9b26 204 }
RobertoO 0:67c50348f842 205
S1933191 2:5093b66c9b26 206 void but1_interrupt()
S1933191 2:5093b66c9b26 207 {
S1933191 2:5093b66c9b26 208 if(but2.read()) {//both buttons are pressed
S1933191 2:5093b66c9b26 209 failure_occurred = true;
S1933191 2:5093b66c9b26 210 }
S1933191 2:5093b66c9b26 211 but1_pressed = true;
S1933191 2:5093b66c9b26 212 pc.printf("Button 1 pressed \n\r");
S1933191 2:5093b66c9b26 213 }
S1933191 2:5093b66c9b26 214
S1933191 2:5093b66c9b26 215 void but2_interrupt()
S1933191 2:5093b66c9b26 216 {
S1933191 2:5093b66c9b26 217 if(but1.read()) {//both buttons are pressed
S1933191 2:5093b66c9b26 218 failure_occurred = true;
S1933191 2:5093b66c9b26 219 }
S1933191 2:5093b66c9b26 220 but2_pressed = true;
S1933191 2:5093b66c9b26 221 pc.printf("Button 2 pressed \n\r");
S1933191 2:5093b66c9b26 222 }
S1933191 2:5093b66c9b26 223
S1933191 2:5093b66c9b26 224 void main_loop()
S1933191 2:5093b66c9b26 225 {
S1933191 2:5093b66c9b26 226 measure_signals();
S1933191 2:5093b66c9b26 227 state_machine();
S1933191 2:5093b66c9b26 228 }
RobertoO 0:67c50348f842 229
RobertoO 0:67c50348f842 230 int main()
RobertoO 0:67c50348f842 231 {
RobertoO 0:67c50348f842 232 pc.baud(115200);
S1933191 2:5093b66c9b26 233 pc.printf("Executing main()... \r\n");
S1933191 2:5093b66c9b26 234 state = s_idle;
S1933191 2:5093b66c9b26 235
RobertoO 0:67c50348f842 236
S1933191 2:5093b66c9b26 237 but1.fall(&but1_interrupt);
S1933191 2:5093b66c9b26 238 but2.fall(&but2_interrupt);
S1933191 2:5093b66c9b26 239 loop_ticker.attach(&main_loop, dt);
S1933191 2:5093b66c9b26 240 pc.printf("main_loop is running\n\r");
S1933191 2:5093b66c9b26 241
S1933191 2:5093b66c9b26 242 }
S1933191 2:5093b66c9b26 243