Tess Groeneveld / Motoraansturingvoortweemotorenmetbeginwaarde

jkfodk

Dependencies:   Encoder MODSERIAL mbed

main.cpp@12:32dad0e2f141, 2013-11-01 (annotated)

Committer:
Tess
Date:
Fri Nov 01 09:40:06 2013 +0000
Revision:
12:32dad0e2f141
Parent:
11:2b4f12230ff0 
Version 5: working script!!! With the real PID constants. These constants needs to be *0.01

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Tess 0:f492ec86159e 1 #include "mbed.h"
Tess 0:f492ec86159e 2 #include "encoder.h"
Tess 0:f492ec86159e 3 #include "MODSERIAL.h"
Tess 0:f492ec86159e 4
Tess 0:f492ec86159e 5 /*******************************************************************************
Tess 0:f492ec86159e 6 * *
Tess 0:f492ec86159e 7 * Code can be found at http://mbed.org/users/vsluiter/code/BMT-K9-Regelaar/ *
Tess 0:f492ec86159e 8 * *
Tess 0:f492ec86159e 9 ********************************************************************************/
Tess 0:f492ec86159e 10
Tess 0:f492ec86159e 11 /** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/
Tess 0:f492ec86159e 12 void keep_in_range(float * in, float min, float max);
Tess 0:f492ec86159e 13
Tess 0:f492ec86159e 14 /** variable to show when a new loop can be started*/
Tess 0:f492ec86159e 15 /** volatile means that it can be changed in an */
Tess 0:f492ec86159e 16 /** interrupt routine, and that that change is vis-*/
Tess 0:f492ec86159e 17 /** ible in the main loop. */
Tess 0:f492ec86159e 18
Tess 0:f492ec86159e 19 volatile bool looptimerflag;
Tess 0:f492ec86159e 20
Tess 0:f492ec86159e 21 /** function called by Ticker "looptimer" */
Tess 0:f492ec86159e 22 /** variable 'looptimerflag' is set to 'true' */
Tess 0:f492ec86159e 23 /** each time the looptimer expires. */
Tess 0:f492ec86159e 24 void setlooptimerflag(void)
Tess 0:f492ec86159e 25 {
Tess 0:f492ec86159e 26 looptimerflag = true;
Tess 0:f492ec86159e 27 }
Tess 0:f492ec86159e 28
Tess 0:f492ec86159e 29 int main()
Tess 0:f492ec86159e 30 {
Tess 0:f492ec86159e 31 //LOCAL VARIABLES
Tess 0:f492ec86159e 32 /*Potmeter input*/
Tess 0:f492ec86159e 33 AnalogIn potmeterA(PTC2);
Tess 0:f492ec86159e 34 AnalogIn potmeterB(PTB2);
Tess 0:f492ec86159e 35 /* Encoder, using my encoder library */
Tess 0:f492ec86159e 36 /* First pin should be PTDx or PTAx */
Tess 0:f492ec86159e 37 /* because those pins can be used as */
Tess 0:f492ec86159e 38 /* InterruptIn */
Tess 0:f492ec86159e 39 Encoder motorA(PTD4,PTC8);
Tess 0:f492ec86159e 40 Encoder motorB(PTD0,PTD2);
Tess 0:f492ec86159e 41 /* MODSERIAL to get non-blocking Serial*/
Tess 0:f492ec86159e 42 MODSERIAL pc(USBTX,USBRX);
Tess 0:f492ec86159e 43 /* PWM control to motor */
Tess 0:f492ec86159e 44 PwmOut pwm_motorA(PTA12);
Tess 0:f492ec86159e 45 PwmOut pwm_motorB(PTA5);
Tess 0:f492ec86159e 46 /* Direction pin */
Tess 0:f492ec86159e 47 DigitalOut motordirA(PTD3);
Tess 0:f492ec86159e 48 DigitalOut motordirB(PTD1);
Tess 0:f492ec86159e 49 /* variable to store setpoint in */
Tess 0:f492ec86159e 50 float setpointA;
Tess 0:f492ec86159e 51 float setpointB;
Tess 2:83dd9068b6c5 52 float setpoint_beginA;
Tess 2:83dd9068b6c5 53 float setpoint_beginB;
Tess 2:83dd9068b6c5 54 float setpoint_rechtsonderA;
Tess 2:83dd9068b6c5 55 float setpoint_rechtsonderB;
Tess 9:c49363372755 56
Tess 0:f492ec86159e 57 /* variable to store pwm value in*/
Tess 0:f492ec86159e 58 float pwm_to_motorA;
vsluiter 4:8344a3edd96c 59 float pwm_to_begin_motorA = 0;
vsluiter 4:8344a3edd96c 60 float pwm_to_begin_motorB = 0;
Tess 0:f492ec86159e 61 float pwm_to_motorB;
Tess 2:83dd9068b6c5 62 float pwm_to_rechtsonder_motorA;
Tess 2:83dd9068b6c5 63 float pwm_to_rechtsonder_motorB;
Tess 9:c49363372755 64
Tess 9:c49363372755 65 const float dt = 0.002;
Tess 12:32dad0e2f141 66 float Kp = 0.001; //0.0208
Tess 12:32dad0e2f141 67 float Kd = 0.00004342; //0.0006897
Tess 9:c49363372755 68 float error_t0_A = 0;
Tess 9:c49363372755 69 float error_t0_B = 0;
Tess 9:c49363372755 70 float error_ti_A;
Tess 9:c49363372755 71 float error_ti_B;
Tess 9:c49363372755 72 float P_regelaar_A;
Tess 9:c49363372755 73 float P_regelaar_B;
Tess 9:c49363372755 74 float D_regelaar_A;
Tess 9:c49363372755 75 float D_regelaar_B;
Tess 9:c49363372755 76 float output_regelaar_A;
Tess 9:c49363372755 77 float output_regelaar_B;
Tess 11:2b4f12230ff0 78
Tess 1:7cafc9042056 79 int32_t positionmotorA_t0;
Tess 1:7cafc9042056 80 int32_t positionmotorB_t0;
Tess 1:7cafc9042056 81 int32_t positionmotorA_t_1;
Tess 1:7cafc9042056 82 int32_t positionmotorB_t_1;
Tess 2:83dd9068b6c5 83 int32_t positiondifference_motorA;
Tess 2:83dd9068b6c5 84 int32_t positiondifference_motorB;
Tess 1:7cafc9042056 85
Tess 0:f492ec86159e 86 //START OF CODE
Tess 0:f492ec86159e 87
Tess 11:2b4f12230ff0 88 /*Set the baudrate (use this number in RealTerm too!) */
Tess 11:2b4f12230ff0 89 pc.baud(921600);
Tess 0:f492ec86159e 90
Tess 11:2b4f12230ff0 91 // in dit stukje code zorgen we ervoor dat de arm gaat draaien naar rechts en stopt als het tegen het frame komt. Eerst motor B botsen dan motor A botsen.
Tess 11:2b4f12230ff0 92 // motor B zit onder en motor A zit boven en dus op zijn kop (en dus setpoint moet - zijn).
Tess 7:1f88215b504c 93
Tess 11:2b4f12230ff0 94 motordirB.write(0);
Tess 11:2b4f12230ff0 95 pwm_motorB.write(.08);
Tess 11:2b4f12230ff0 96 positionmotorB_t0 = motorB.getPosition();
Tess 11:2b4f12230ff0 97 do {
Tess 11:2b4f12230ff0 98 wait(0.2);
Tess 11:2b4f12230ff0 99 positionmotorB_t_1 = positionmotorB_t0 ;
Tess 11:2b4f12230ff0 100 positionmotorB_t0 = motorB.getPosition();
Tess 11:2b4f12230ff0 101 positiondifference_motorB = abs(positionmotorB_t0 - positionmotorB_t_1);
Tess 11:2b4f12230ff0 102 } while(positiondifference_motorB > 10);
Tess 11:2b4f12230ff0 103 motorB.setPosition(0);
Tess 11:2b4f12230ff0 104 pwm_motorB.write(0);
Tess 1:7cafc9042056 105
Tess 11:2b4f12230ff0 106 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 7:1f88215b504c 107
Tess 11:2b4f12230ff0 108 motordirA.write(1);
Tess 11:2b4f12230ff0 109 pwm_motorA.write(.08);
Tess 11:2b4f12230ff0 110 positionmotorA_t0 = motorA.getPosition();
Tess 11:2b4f12230ff0 111 do {
Tess 11:2b4f12230ff0 112 wait(0.2);
Tess 11:2b4f12230ff0 113 positionmotorA_t_1 = positionmotorA_t0 ;
Tess 11:2b4f12230ff0 114 positionmotorA_t0 = motorA.getPosition();
Tess 11:2b4f12230ff0 115 positiondifference_motorA = abs(positionmotorA_t0 - positionmotorA_t_1);
Tess 11:2b4f12230ff0 116 } while(positiondifference_motorA > 10);
Tess 11:2b4f12230ff0 117 motorA.setPosition(0);
Tess 11:2b4f12230ff0 118 pwm_motorA.write(0);
Tess 2:83dd9068b6c5 119
Tess 11:2b4f12230ff0 120 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 7:1f88215b504c 121
Tess 11:2b4f12230ff0 122 // Hierna willen we de motor van zijn alleruiterste positie naar de x-as hebben. Hiervoor moet motor A eerst op de x-as worden gezet. Hiervoor moet motor A 4.11 graden (63) naar links.
Tess 2:83dd9068b6c5 123
Tess 11:2b4f12230ff0 124 motordirA.write(0);
Tess 11:2b4f12230ff0 125 pwm_motorA.write(.08);
Tess 11:2b4f12230ff0 126 do {
Tess 11:2b4f12230ff0 127 setpoint_beginA = -63; // x-as
Tess 11:2b4f12230ff0 128 pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001); // + omdat men met een negatieve hoekverdraaiing werkt.
Tess 11:2b4f12230ff0 129 wait(0.2);
Tess 11:2b4f12230ff0 130 keep_in_range(&pwm_to_begin_motorA, -1, 1 );
Tess 11:2b4f12230ff0 131 motordirA.write(0);
Tess 11:2b4f12230ff0 132 pwm_motorA.write(pwm_to_begin_motorA);
Tess 11:2b4f12230ff0 133 } while(pwm_to_begin_motorA <= 0);
Tess 11:2b4f12230ff0 134 motorA.setPosition(0);
Tess 11:2b4f12230ff0 135 pwm_motorA.write(0);
Tess 10:9d7110f25908 136
Tess 11:2b4f12230ff0 137 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 7:1f88215b504c 138
Tess 11:2b4f12230ff0 139 // hierna moet motor A naar de rechtsonder A4. Motor A 532.
Tess 9:c49363372755 140
Tess 11:2b4f12230ff0 141 motordirA.write(0);
Tess 11:2b4f12230ff0 142 pwm_motorA.write(0.08);
Tess 11:2b4f12230ff0 143 do {
Tess 11:2b4f12230ff0 144 setpoint_rechtsonderA = -532; // rechtsonder positie A4
Tess 11:2b4f12230ff0 145 pwm_to_rechtsonder_motorA = abs((setpoint_rechtsonderA + motorA.getPosition()) *.001);
Tess 11:2b4f12230ff0 146 wait(0.2);
Tess 11:2b4f12230ff0 147 keep_in_range(&pwm_to_rechtsonder_motorA, -1, 1 );
Tess 11:2b4f12230ff0 148 motordirA.write(0);
Tess 11:2b4f12230ff0 149 pwm_motorA.write(pwm_to_rechtsonder_motorA);
Tess 11:2b4f12230ff0 150 } while(pwm_to_rechtsonder_motorA <= 0);
Tess 11:2b4f12230ff0 151 pwm_motorA.write(0);
Tess 9:c49363372755 152
Tess 11:2b4f12230ff0 153 wait(1);
Tess 10:9d7110f25908 154
Tess 11:2b4f12230ff0 155 // Hierna moet motor B 21.6 (192) graden naar links om naar x-as te gaan.
Tess 2:83dd9068b6c5 156
Tess 11:2b4f12230ff0 157 motordirB.write(1);
Tess 11:2b4f12230ff0 158 pwm_motorB.write(.08);
Tess 11:2b4f12230ff0 159 do {
Tess 11:2b4f12230ff0 160 setpoint_beginB = 192; // x-as
Tess 11:2b4f12230ff0 161 pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
Tess 11:2b4f12230ff0 162 wait(0.2);
Tess 11:2b4f12230ff0 163 keep_in_range(&pwm_to_begin_motorB, -1, 1 );
Tess 11:2b4f12230ff0 164 motordirB.write(1);
Tess 11:2b4f12230ff0 165 pwm_motorB.write(pwm_to_begin_motorB);
Tess 11:2b4f12230ff0 166 } while(pwm_to_begin_motorB <= 0);
Tess 11:2b4f12230ff0 167 motorB.setPosition(0);
Tess 11:2b4f12230ff0 168 pwm_motorB.write(0);
Tess 10:9d7110f25908 169
Tess 11:2b4f12230ff0 170 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 10:9d7110f25908 171
Tess 11:2b4f12230ff0 172 // Hierna moet motor B van x-as naar de rechtsonder A4 positie. Motor B 460.
Tess 9:c49363372755 173
Tess 11:2b4f12230ff0 174 motordirB.write(1);
Tess 11:2b4f12230ff0 175 pwm_motorB.write(0.08);
Tess 11:2b4f12230ff0 176 do {
Tess 11:2b4f12230ff0 177 setpoint_rechtsonderB = 460; // rechtsonder positie A4
Tess 11:2b4f12230ff0 178 pwm_to_rechtsonder_motorB = abs((setpoint_rechtsonderB - motorB.getPosition()) *.001);
Tess 11:2b4f12230ff0 179 wait(0.2);
Tess 11:2b4f12230ff0 180 keep_in_range(&pwm_to_rechtsonder_motorB, -1, 1 );
Tess 11:2b4f12230ff0 181 motordirB.write(1);
Tess 11:2b4f12230ff0 182 pwm_motorB.write(pwm_to_rechtsonder_motorB);
Tess 11:2b4f12230ff0 183 } while(pwm_to_rechtsonder_motorB <= 0);
Tess 11:2b4f12230ff0 184 pwm_motorB.write(0);
Tess 10:9d7110f25908 185
Tess 11:2b4f12230ff0 186 wait(1);
Tess 0:f492ec86159e 187
Tess 11:2b4f12230ff0 188 // Nu zijn de motoren gekalibreed en staan ze op de startpositie.
Tess 11:2b4f12230ff0 189 // Hierna het script dat EMG wordt omgezet in een positie verandering
Tess 11:2b4f12230ff0 190
Tess 11:2b4f12230ff0 191 /*Create a ticker, and let it call the */
Tess 11:2b4f12230ff0 192 /*function 'setlooptimerflag' every 0.01s */
Tess 11:2b4f12230ff0 193 Ticker looptimer;
Tess 11:2b4f12230ff0 194 looptimer.attach(setlooptimerflag,0.01);
Tess 11:2b4f12230ff0 195
Tess 11:2b4f12230ff0 196 //INFINITE LOOP
Tess 11:2b4f12230ff0 197 while(1) {
Tess 11:2b4f12230ff0 198
Tess 11:2b4f12230ff0 199 while(looptimerflag != true);
Tess 11:2b4f12230ff0 200 looptimerflag = false;
Tess 8:62e968f78878 201
Tess 11:2b4f12230ff0 202 // hier EMG
Tess 11:2b4f12230ff0 203 //setpointA = (potmeterA.read()-0.09027)*(631); // bereik van 71 graden dit afhankelijk van waar nul punt zit en waar heel wil. Dus afh. van EMG lezen bij EMG wordt 0.5 - 0.09027
Tess 11:2b4f12230ff0 204 //setpointB = (potmeterB.read())*(415); // bereik van 46.7 graden
Tess 11:2b4f12230ff0 205 //pc.printf("s: %f, %d ", setpointA, motorA.getPosition());
Tess 11:2b4f12230ff0 206 //pc.printf("s: %f, %d ", setpointB, motorB.getPosition());
Tess 11:2b4f12230ff0 207 setpointA = (potmeterA.read() - 0.5)*(631/2);
Tess 11:2b4f12230ff0 208 setpointB = (potmeterB.read() - 0.5) * (871/2);
Tess 8:62e968f78878 209
Tess 11:2b4f12230ff0 210 // motor A moet de hoek altijd binnen 53.4 tot en met 124.3 graden liggen
Tess 11:2b4f12230ff0 211 // motor B moet de hoek altijd binnen 30.2 tot en met -16.5 graden liggen
Tess 11:2b4f12230ff0 212 /*keep_in_range(&setpointA, -1105, -474); // voor motor moet bereik zijn -1105 tot -474
Tess 11:2b4f12230ff0 213 keep_in_range(&setpointB, -147, 269); // voor motor moet bereik zijn -147 tot 269
Tess 11:2b4f12230ff0 214 */
Tess 11:2b4f12230ff0 215
Tess 11:2b4f12230ff0 216 //PD regelaar voor motor A
Tess 11:2b4f12230ff0 217 wait(dt);
Tess 11:2b4f12230ff0 218 error_ti_A = setpointA - motorA.getPosition();
Tess 11:2b4f12230ff0 219 P_regelaar_A = Kp * error_ti_A;
Tess 11:2b4f12230ff0 220 D_regelaar_A = Kd * ((error_ti_A - error_t0_A) / dt);
Tess 11:2b4f12230ff0 221 error_t0_A = error_ti_A;
Tess 11:2b4f12230ff0 222 output_regelaar_A = P_regelaar_A + D_regelaar_A;
Tess 9:c49363372755 223
Tess 11:2b4f12230ff0 224 //PD regelaar voor motor B
Tess 11:2b4f12230ff0 225 wait(dt);
Tess 11:2b4f12230ff0 226 error_ti_B = setpointB - motorB.getPosition();
Tess 11:2b4f12230ff0 227 P_regelaar_B = Kp * error_ti_B;
Tess 11:2b4f12230ff0 228 D_regelaar_B = Kd * ((error_ti_B - error_t0_B) / dt);
Tess 11:2b4f12230ff0 229 error_t0_B = error_ti_B;
Tess 11:2b4f12230ff0 230 output_regelaar_B = P_regelaar_B + D_regelaar_B;
Tess 9:c49363372755 231
Tess 11:2b4f12230ff0 232 /* This is a PID-action! store in pwm_to_motor */
Tess 11:2b4f12230ff0 233 pwm_to_motorA = output_regelaar_A;
Tess 11:2b4f12230ff0 234 pwm_to_motorB = output_regelaar_B;
Tess 9:c49363372755 235
Tess 11:2b4f12230ff0 236 keep_in_range(&pwm_to_motorA, -1,1);
Tess 11:2b4f12230ff0 237 keep_in_range(&pwm_to_motorB, -1,1);
Tess 8:62e968f78878 238
Tess 11:2b4f12230ff0 239 if(pwm_to_motorA > 0)
Tess 11:2b4f12230ff0 240 motordirA.write(1);
Tess 11:2b4f12230ff0 241 else
Tess 11:2b4f12230ff0 242 motordirA.write(0);
Tess 11:2b4f12230ff0 243 if(pwm_to_motorB > 0)
Tess 11:2b4f12230ff0 244 motordirB.write(1);
Tess 11:2b4f12230ff0 245 else
Tess 11:2b4f12230ff0 246 motordirB.write(0);
Tess 11:2b4f12230ff0 247
Tess 11:2b4f12230ff0 248 pwm_motorA.write(abs(pwm_to_motorA));
Tess 11:2b4f12230ff0 249 pwm_motorB.write(abs(pwm_to_motorB));
Tess 8:62e968f78878 250 }
Tess 0:f492ec86159e 251 }
Tess 0:f492ec86159e 252
Tess 0:f492ec86159e 253
Tess 0:f492ec86159e 254 void keep_in_range(float * in, float min, float max)
Tess 0:f492ec86159e 255 {
Tess 0:f492ec86159e 256 *in > min ? *in < max? : *in = max: *in = min;
Tess 0:f492ec86159e 257 }
Tess 0:f492ec86159e 258
Tess 0:f492ec86159e 259
Tess 0:f492ec86159e 260
Tess 7:1f88215b504c 261
Tess 7:1f88215b504c 262
Tess 7:1f88215b504c 263
Tess 7:1f88215b504c 264