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@10:9d7110f25908, 2013-10-31 (annotated)

Committer:
Tess
Date:
Thu Oct 31 13:45:13 2013 +0000
Revision:
10:9d7110f25908
Parent:
9:c49363372755
Child:
11:2b4f12230ff0
doesn't work, something with the port

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