Tess Groeneveld / Motoraansturingvoortweemotorenmetbeginwaarde

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Encoder MODSERIAL mbed

main.cpp@9:c49363372755, 2013-10-31 (annotated)

Committer:
Tess
Date:
Thu Oct 31 12:48:12 2013 +0000
Revision:
9:c49363372755
Parent:
8:62e968f78878
Child:
10:9d7110f25908
Port PTC2 not working

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 9:c49363372755 66 float Kp = 0.001; //0.0113
Tess 9:c49363372755 67 float Ki = 0.0759;
Tess 9:c49363372755 68 float Kd = 0.0004342;
Tess 9:c49363372755 69 float error_t0_A = 0;
Tess 9:c49363372755 70 float error_t0_B = 0;
Tess 9:c49363372755 71 float error_ti_A;
Tess 9:c49363372755 72 float error_ti_B;
Tess 9:c49363372755 73 float error_t_1_A;
Tess 9:c49363372755 74 float error_t_1_B;
Tess 9:c49363372755 75 float P_regelaar_A;
Tess 9:c49363372755 76 float P_regelaar_B;
Tess 9:c49363372755 77 float I_regelaar_A;
Tess 9:c49363372755 78 float I_regelaar_B;
Tess 9:c49363372755 79 float D_regelaar_A;
Tess 9:c49363372755 80 float D_regelaar_B;
Tess 9:c49363372755 81 float output_regelaar_A;
Tess 9:c49363372755 82 float output_regelaar_B;
Tess 9:c49363372755 83 float integral_i_A;
Tess 9:c49363372755 84 float integral_i_B;
Tess 9:c49363372755 85 float integral_0_A = 0;
Tess 9:c49363372755 86 float integral_0_B = 0;
Tess 9:c49363372755 87
Tess 1:7cafc9042056 88 int32_t positionmotorA_t0;
Tess 1:7cafc9042056 89 int32_t positionmotorB_t0;
Tess 1:7cafc9042056 90 int32_t positionmotorA_t_1;
Tess 1:7cafc9042056 91 int32_t positionmotorB_t_1;
Tess 2:83dd9068b6c5 92 int32_t positiondifference_motorA;
Tess 2:83dd9068b6c5 93 int32_t positiondifference_motorB;
Tess 1:7cafc9042056 94
Tess 0:f492ec86159e 95 //START OF CODE
Tess 0:f492ec86159e 96
Tess 5:512dd4044486 97 /*Set the baudrate (use this number in RealTerm too!) */
Tess 0:f492ec86159e 98 pc.baud(921600);
Tess 0:f492ec86159e 99
Tess 7:1f88215b504c 100 // 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 7:1f88215b504c 101 // motor B zit onder en motor A zit boven en dus op zijn kop (en dus setpoint moet - zijn).
Tess 7:1f88215b504c 102
Tess 7:1f88215b504c 103 motordirB.write(0);
Tess 2:83dd9068b6c5 104 pwm_motorB.write(.08);
Tess 1:7cafc9042056 105 positionmotorB_t0 = motorB.getPosition();
Tess 1:7cafc9042056 106 do {
Tess 2:83dd9068b6c5 107 wait(0.2);
Tess 7:1f88215b504c 108 positionmotorB_t_1 = positionmotorB_t0 ;
Tess 1:7cafc9042056 109 positionmotorB_t0 = motorB.getPosition();
Tess 2:83dd9068b6c5 110 positiondifference_motorB = abs(positionmotorB_t0 - positionmotorB_t_1);
Tess 2:83dd9068b6c5 111 } while(positiondifference_motorB > 10);
Tess 2:83dd9068b6c5 112 motorB.setPosition(0);
Tess 2:83dd9068b6c5 113 pwm_motorB.write(0);
Tess 1:7cafc9042056 114
Tess 7:1f88215b504c 115 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 7:1f88215b504c 116
Tess 2:83dd9068b6c5 117 motordirA.write(1);
Tess 2:83dd9068b6c5 118 pwm_motorA.write(.08);
Tess 2:83dd9068b6c5 119 positionmotorA_t0 = motorA.getPosition();
Tess 2:83dd9068b6c5 120 do {
Tess 2:83dd9068b6c5 121 wait(0.2);
Tess 2:83dd9068b6c5 122 positionmotorA_t_1 = positionmotorA_t0 ;
Tess 2:83dd9068b6c5 123 positionmotorA_t0 = motorA.getPosition();
Tess 2:83dd9068b6c5 124 positiondifference_motorA = abs(positionmotorA_t0 - positionmotorA_t_1);
Tess 2:83dd9068b6c5 125 } while(positiondifference_motorA > 10);
Tess 2:83dd9068b6c5 126 motorA.setPosition(0);
Tess 2:83dd9068b6c5 127 pwm_motorA.write(0);
Tess 2:83dd9068b6c5 128
Tess 7:1f88215b504c 129 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 7:1f88215b504c 130
Tess 7:1f88215b504c 131 // 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 9:c49363372755 132
Tess 7:1f88215b504c 133 motordirA.write(0);
Tess 7:1f88215b504c 134 pwm_motorA.write(.08);
Tess 5:512dd4044486 135 do {
Tess 9:c49363372755 136 setpoint_beginA = -63; // x-as
Tess 7:1f88215b504c 137 pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001); // + omdat men met een negatieve hoekverdraaiing werkt.
Tess 8:62e968f78878 138 wait(0.2);
Tess 2:83dd9068b6c5 139 keep_in_range(&pwm_to_begin_motorA, -1, 1 );
Tess 2:83dd9068b6c5 140 motordirA.write(0);
Tess 5:512dd4044486 141 pwm_motorA.write(pwm_to_begin_motorA);
Tess 8:62e968f78878 142 } while(pwm_to_begin_motorA <= 0);
Tess 3:c4df318913b8 143 motorA.setPosition(0);
Tess 3:c4df318913b8 144 pwm_motorA.write(0);
Tess 2:83dd9068b6c5 145
Tess 7:1f88215b504c 146 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 7:1f88215b504c 147
Tess 9:c49363372755 148 // hierna moet motor A naar de rechtsonder A4. Motor A 532.
Tess 9:c49363372755 149
Tess 9:c49363372755 150 motordirA.write(0);
Tess 9:c49363372755 151 pwm_motorA.write(0.08);
Tess 9:c49363372755 152 do {
Tess 9:c49363372755 153 setpoint_beginA = -532; // rechtsonder positie A4
Tess 9:c49363372755 154 pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001);
Tess 9:c49363372755 155 wait(0.2);
Tess 9:c49363372755 156 keep_in_range(&pwm_to_begin_motorA, -1, 1 );
Tess 9:c49363372755 157 motordirA.write(0);
Tess 9:c49363372755 158 pwm_motorA.write(pwm_to_begin_motorA);
Tess 9:c49363372755 159 } while(pwm_to_begin_motorA <= 0);
Tess 9:c49363372755 160 pwm_motorA.write(0);
Tess 9:c49363372755 161
Tess 9:c49363372755 162 wait(1);
Tess 9:c49363372755 163
Tess 9:c49363372755 164 // Hierna moet motor B 21.6 (192) graden naar links om naar x-as te gaan.
Tess 9:c49363372755 165
Tess 7:1f88215b504c 166 motordirB.write(1);
Tess 7:1f88215b504c 167 pwm_motorB.write(.08);
Tess 7:1f88215b504c 168 do {
Tess 9:c49363372755 169 setpoint_beginB = 192; // x-as
Tess 7:1f88215b504c 170 pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
Tess 8:62e968f78878 171 wait(0.2);
Tess 7:1f88215b504c 172 keep_in_range(&pwm_to_begin_motorB, -1, 1 );
Tess 3:c4df318913b8 173 motordirB.write(1);
Tess 5:512dd4044486 174 pwm_motorB.write(pwm_to_begin_motorB);
Tess 8:62e968f78878 175 } while(pwm_to_begin_motorB <= 0);
Tess 3:c4df318913b8 176 motorB.setPosition(0);
Tess 3:c4df318913b8 177 pwm_motorB.write(0);
Tess 2:83dd9068b6c5 178
Tess 7:1f88215b504c 179 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 9:c49363372755 180
Tess 9:c49363372755 181 // Hierna moet motor B van x-as naar de rechtsonder A4 positie. Motor B 460.
Tess 9:c49363372755 182
Tess 7:1f88215b504c 183 motordirB.write(1);
Tess 7:1f88215b504c 184 pwm_motorB.write(0.08);
Tess 7:1f88215b504c 185 do {
Tess 9:c49363372755 186 setpoint_beginB = 460; // rechtsonder positie A4
Tess 7:1f88215b504c 187 pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
Tess 8:62e968f78878 188 wait(0.2);
Tess 7:1f88215b504c 189 keep_in_range(&pwm_to_begin_motorB, -1, 1 );
Tess 7:1f88215b504c 190 motordirB.write(1);
Tess 7:1f88215b504c 191 pwm_motorB.write(pwm_to_begin_motorB);
Tess 9:c49363372755 192 } while(pwm_to_begin_motorB <= 0);
Tess 3:c4df318913b8 193 pwm_motorB.write(0);
Tess 9:c49363372755 194
Tess 9:c49363372755 195 wait(1);
Tess 9:c49363372755 196
Tess 8:62e968f78878 197 // Nu zijn de motoren gekalibreed en staan ze op de startpositie.
Tess 8:62e968f78878 198 // Hierna het script dat EMG wordt omgezet in een positie verandering
Tess 9:c49363372755 199
Tess 8:62e968f78878 200 /*Create a ticker, and let it call the */
Tess 8:62e968f78878 201 /*function 'setlooptimerflag' every 0.01s */
Tess 8:62e968f78878 202 Ticker looptimer;
Tess 8:62e968f78878 203 looptimer.attach(setlooptimerflag,0.01);
Tess 0:f492ec86159e 204
Tess 8:62e968f78878 205 //INFINITE LOOP
Tess 8:62e968f78878 206 while(1) {
Tess 9:c49363372755 207
Tess 8:62e968f78878 208 while(looptimerflag != true);
Tess 8:62e968f78878 209 looptimerflag = false;
Tess 8:62e968f78878 210
Tess 8:62e968f78878 211 // hier EMG
Tess 9:c49363372755 212 //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 9:c49363372755 213 //setpointB = (potmeterB.read())*(415); // bereik van 46.7 graden
Tess 9:c49363372755 214 //pc.printf("s: %f, %d ", setpointA, motorA.getPosition());
Tess 9:c49363372755 215 //pc.printf("s: %f, %d ", setpointB, motorB.getPosition());
Tess 9:c49363372755 216 setpointA = (potmeterA.read() - 0.5)*(631/2);
Tess 9:c49363372755 217 setpointB = (potmeterB.read() - 0.5) * (871/2);
Tess 8:62e968f78878 218 // motor A moet de hoek altijd binnen 53.4 tot en met 124.3 graden liggen
Tess 8:62e968f78878 219 // motor B moet de hoek altijd binnen 30.2 tot en met -16.5 graden liggen
Tess 8:62e968f78878 220 keep_in_range(&setpointA, -1105, -474); // voor motor moet bereik zijn -1105 tot -474
Tess 8:62e968f78878 221 keep_in_range(&setpointB, -147, 269); // voor motor moet bereik zijn -147 tot 269
Tess 8:62e968f78878 222
Tess 9:c49363372755 223 // PID regelaar voor motor A
Tess 9:c49363372755 224 //wait(dt);
Tess 9:c49363372755 225 //error_ti_A = setpointA - motorA.getPosition();
Tess 9:c49363372755 226 //P_regelaar_A = Kp * error_ti_A;
Tess 9:c49363372755 227 //D_regelaar_A = Kd * ((error_ti_A - error_t0_A) / dt);
Tess 9:c49363372755 228 //integral_i_A = integral_0_A + (error_ti_A * dt);
Tess 9:c49363372755 229 //I_regelaar_A = Ki * integral_i_A;
Tess 9:c49363372755 230 //integral_0_A = integral_i_A;
Tess 9:c49363372755 231 //error_t0_A = error_ti_A;
Tess 9:c49363372755 232 //output_regelaar_A = P_regelaar_A;
Tess 9:c49363372755 233
Tess 9:c49363372755 234 // PID regelaar voor motor B
Tess 9:c49363372755 235 //wait(dt);
Tess 9:c49363372755 236 //error_ti_B = setpointB - motorB.getPosition();
Tess 9:c49363372755 237 //P_regelaar_B = Kp * error_ti_B;
Tess 9:c49363372755 238 //D_regelaar_B = Kd * ((error_ti_B - error_t0_B) / dt);
Tess 9:c49363372755 239 //integral_i_B = integral_0_B + (error_ti_B * dt);
Tess 9:c49363372755 240 //I_regelaar_B = Ki * integral_i_B;
Tess 9:c49363372755 241 //integral_0_B = integral_i_B;
Tess 9:c49363372755 242 //error_t0_B = error_ti_B;
Tess 9:c49363372755 243 //output_regelaar_B = P_regelaar_B;
Tess 9:c49363372755 244
Tess 9:c49363372755 245 /* This is a PID-action! store in pwm_to_motor */
Tess 9:c49363372755 246 pwm_to_motorA = (setpointA - motorA.getPosition())*.001; //output_regelaar_A;
Tess 9:c49363372755 247 pwm_to_motorB = (setpointB - motorB.getPosition())*.001; //output_regelaar_B;
Tess 9:c49363372755 248
Tess 8:62e968f78878 249 keep_in_range(&pwm_to_motorA, -1,1);
Tess 8:62e968f78878 250 keep_in_range(&pwm_to_motorB, -1,1);
Tess 8:62e968f78878 251
Tess 8:62e968f78878 252 if(pwm_to_motorA > 0)
Tess 8:62e968f78878 253 motordirA.write(1);
Tess 8:62e968f78878 254 else
Tess 8:62e968f78878 255 motordirA.write(0);
Tess 8:62e968f78878 256 if(pwm_to_motorB > 0)
Tess 8:62e968f78878 257 motordirB.write(1);
Tess 8:62e968f78878 258 else
Tess 8:62e968f78878 259 motordirB.write(0);
Tess 8:62e968f78878 260
Tess 8:62e968f78878 261 pwm_motorA.write(abs(pwm_to_motorA));
Tess 8:62e968f78878 262 pwm_motorB.write(abs(pwm_to_motorB));
Tess 8:62e968f78878 263 }
Tess 0:f492ec86159e 264 }
Tess 0:f492ec86159e 265
Tess 0:f492ec86159e 266
Tess 0:f492ec86159e 267 void keep_in_range(float * in, float min, float max)
Tess 0:f492ec86159e 268 {
Tess 0:f492ec86159e 269 *in > min ? *in < max? : *in = max: *in = min;
Tess 0:f492ec86159e 270 }
Tess 0:f492ec86159e 271
Tess 0:f492ec86159e 272
Tess 0:f492ec86159e 273
Tess 7:1f88215b504c 274
Tess 7:1f88215b504c 275
Tess 7:1f88215b504c 276
Tess 7:1f88215b504c 277