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@5:512dd4044486, 2013-10-30 (annotated)

Committer:
Tess
Date:
Wed Oct 30 10:10:29 2013 +0000
Revision:
5:512dd4044486
Parent:
4:8344a3edd96c
Child:
6:2cfdda6721ab
version 4; do while loop made but 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 0:f492ec86159e 56 /* variable to store pwm value in*/
Tess 0:f492ec86159e 57 float pwm_to_motorA;
vsluiter 4:8344a3edd96c 58 float pwm_to_begin_motorA = 0;
vsluiter 4:8344a3edd96c 59 float pwm_to_begin_motorB = 0;
Tess 0:f492ec86159e 60 float pwm_to_motorB;
Tess 2:83dd9068b6c5 61 float pwm_to_rechtsonder_motorA;
Tess 2:83dd9068b6c5 62 float pwm_to_rechtsonder_motorB;
Tess 1:7cafc9042056 63 int32_t positionmotorA_t0;
Tess 1:7cafc9042056 64 int32_t positionmotorB_t0;
Tess 1:7cafc9042056 65 int32_t positionmotorA_t_1;
Tess 1:7cafc9042056 66 int32_t positionmotorB_t_1;
Tess 2:83dd9068b6c5 67 int32_t positiondifference_motorA;
Tess 2:83dd9068b6c5 68 int32_t positiondifference_motorB;
Tess 1:7cafc9042056 69
Tess 0:f492ec86159e 70 //START OF CODE
Tess 0:f492ec86159e 71
Tess 5:512dd4044486 72 /*Set the baudrate (use this number in RealTerm too!) */
Tess 0:f492ec86159e 73 pc.baud(921600);
Tess 0:f492ec86159e 74
Tess 5:512dd4044486 75 // 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 0:f492ec86159e 76 motordirB.write(0);
Tess 2:83dd9068b6c5 77 pwm_motorB.write(.08);
Tess 1:7cafc9042056 78 positionmotorB_t0 = motorB.getPosition();
Tess 1:7cafc9042056 79 do {
Tess 2:83dd9068b6c5 80 wait(0.2);
Tess 1:7cafc9042056 81 positionmotorB_t_1 = positionmotorB_t0;
Tess 1:7cafc9042056 82 positionmotorB_t0 = motorB.getPosition();
Tess 2:83dd9068b6c5 83 positiondifference_motorB = abs(positionmotorB_t0 - positionmotorB_t_1);
Tess 2:83dd9068b6c5 84 } while(positiondifference_motorB > 10);
Tess 2:83dd9068b6c5 85 motorB.setPosition(0);
Tess 2:83dd9068b6c5 86 pwm_motorB.write(0);
Tess 1:7cafc9042056 87
Tess 2:83dd9068b6c5 88 motordirA.write(1);
Tess 2:83dd9068b6c5 89 pwm_motorA.write(.08);
Tess 2:83dd9068b6c5 90 positionmotorA_t0 = motorA.getPosition();
Tess 2:83dd9068b6c5 91 do {
Tess 2:83dd9068b6c5 92 wait(0.2);
Tess 2:83dd9068b6c5 93 positionmotorA_t_1 = positionmotorA_t0 ;
Tess 2:83dd9068b6c5 94 positionmotorA_t0 = motorA.getPosition();
Tess 2:83dd9068b6c5 95 positiondifference_motorA = abs(positionmotorA_t0 - positionmotorA_t_1);
Tess 2:83dd9068b6c5 96 } while(positiondifference_motorA > 10);
Tess 2:83dd9068b6c5 97 motorA.setPosition(0);
Tess 2:83dd9068b6c5 98 pwm_motorA.write(0);
Tess 2:83dd9068b6c5 99
Tess 5:512dd4044486 100 // Hierna willen we de motor van zijn aller uiterste 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 5:512dd4044486 101 // Hierna moet motor B 21.6 (192) graden naar links. Dus eerst motor A en dan motor B.
Tess 1:7cafc9042056 102
Tess 5:512dd4044486 103 Ticker looptimer;
Tess 5:512dd4044486 104 looptimer.attach(setlooptimerflag,0.01);
Tess 0:f492ec86159e 105
Tess 5:512dd4044486 106 do {
Tess 5:512dd4044486 107 //pwm_to_begin_motorA = abs((setpoint_beginA - motorA.getPosition()) *.001);
Tess 5:512dd4044486 108 } while(pwm_to_begin_motorA <= 0.001); {
Tess 0:f492ec86159e 109 while(looptimerflag != true);
Tess 0:f492ec86159e 110 looptimerflag = false;
Tess 5:512dd4044486 111 setpoint_beginA = -1000; //-63 negatief omdat de motor op zijn kop hangt
Tess 5:512dd4044486 112 pwm_to_begin_motorA = abs((setpoint_beginA - motorA.getPosition()) *.001);
Tess 2:83dd9068b6c5 113 keep_in_range(&pwm_to_begin_motorA, -1, 1 );
Tess 2:83dd9068b6c5 114 motordirA.write(0);
Tess 5:512dd4044486 115 pwm_motorA.write(pwm_to_begin_motorA);
Tess 2:83dd9068b6c5 116 }
Tess 3:c4df318913b8 117 motorA.setPosition(0);
Tess 3:c4df318913b8 118 pwm_motorA.write(0);
Tess 2:83dd9068b6c5 119
Tess 5:512dd4044486 120 do{
Tess 5:512dd4044486 121 pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
Tess 5:512dd4044486 122 }
Tess 5:512dd4044486 123 while(pwm_to_begin_motorB <= 0.001); {
Tess 3:c4df318913b8 124 while(looptimerflag != true);
Tess 3:c4df318913b8 125 looptimerflag = false;
Tess 5:512dd4044486 126 setpoint_beginB = 1000; //192
Tess 3:c4df318913b8 127 keep_in_range(&pwm_to_begin_motorB, -1,1);
Tess 3:c4df318913b8 128 motordirB.write(1);
Tess 5:512dd4044486 129 pwm_motorB.write(pwm_to_begin_motorB);
Tess 3:c4df318913b8 130 }
Tess 3:c4df318913b8 131 motorB.setPosition(0);
Tess 3:c4df318913b8 132 pwm_motorB.write(0);
Tess 2:83dd9068b6c5 133
Tess 5:512dd4044486 134 // nu naar positie rechtsonderhoek A4 deze is voor motor A 532 en voor motor B 460
Tess 3:c4df318913b8 135
Tess 3:c4df318913b8 136 //while(0) ///1
Tess 3:c4df318913b8 137 //{
Tess 3:c4df318913b8 138 // while(looptimerflag != true);
Tess 3:c4df318913b8 139 //
Tess 3:c4df318913b8 140 // looptimerflag = false;
Tess 3:c4df318913b8 141
Tess 3:c4df318913b8 142 // setpoint_rechtsonderA = 531; // naar rechter onderhoek
Tess 3:c4df318913b8 143 // pwm_to_rechtsonder_motorA = setpoint_rechtsonderA *.001;
Tess 3:c4df318913b8 144 // keep_in_range(&pwm_to_rechtsonder_motorA, -1, 1 );
Tess 3:c4df318913b8 145 // if(pwm_to_rechtsonder_motorA > 0.531) {
Tess 3:c4df318913b8 146 // motordirA.write(0);
Tess 3:c4df318913b8 147 // pwm_motorA.write(0);
Tess 3:c4df318913b8 148 // } else
Tess 3:c4df318913b8 149 // motordirA.write(1);
Tess 3:c4df318913b8 150 // pwm_motorA.write(abs(pwm_to_rechtsonder_motorA));
Tess 3:c4df318913b8 151
Tess 3:c4df318913b8 152 // while(looptimerflag != true);
Tess 3:c4df318913b8 153 // looptimerflag = false;
Tess 2:83dd9068b6c5 154
Tess 3:c4df318913b8 155 // setpoint_rechtsonderB = 460;
Tess 3:c4df318913b8 156 // pwm_to_rechtsonder_motorB = setpoint_rechtsonderB *.001;
Tess 3:c4df318913b8 157 // keep_in_range(&pwm_to_rechtsonder_motorB, -1,1);
Tess 3:c4df318913b8 158 // if(pwm_to_rechtsonder_motorB > 0.460) {
Tess 3:c4df318913b8 159 // motordirB.write(0);
Tess 3:c4df318913b8 160 // pwm_motorB.write(0);
Tess 3:c4df318913b8 161 // } else
Tess 3:c4df318913b8 162 // motordirB.write(1);
Tess 3:c4df318913b8 163 // pwm_motorB.write(abs(pwm_to_rechtsonder_motorB));
Tess 3:c4df318913b8 164
Tess 3:c4df318913b8 165 //}
Tess 0:f492ec86159e 166
Tess 3:c4df318913b8 167 while((pwm_to_begin_motorA >= 0)&&(pwm_to_begin_motorB >= 0)) {
Tess 3:c4df318913b8 168 while(looptimerflag != true);
Tess 3:c4df318913b8 169 looptimerflag = false;
Tess 3:c4df318913b8 170
Tess 3:c4df318913b8 171 setpoint_beginA = 1000;//532 // naar rechteronderhoek A4
Tess 3:c4df318913b8 172 pwm_to_begin_motorA = (setpoint_beginA - motorA.getPosition()) *.001;
Tess 3:c4df318913b8 173 keep_in_range(&pwm_to_begin_motorA, -1, 1 );
Tess 2:83dd9068b6c5 174 motordirA.write(0);
Tess 3:c4df318913b8 175 pwm_motorA.write(abs(pwm_to_begin_motorA));
Tess 2:83dd9068b6c5 176
Tess 3:c4df318913b8 177 while(looptimerflag != true);
Tess 3:c4df318913b8 178 looptimerflag = false;
Tess 2:83dd9068b6c5 179
Tess 3:c4df318913b8 180 setpoint_beginB = 1000; //460
Tess 3:c4df318913b8 181 pwm_to_begin_motorB = (setpoint_beginB - motorB.getPosition()) *.001;
Tess 3:c4df318913b8 182 keep_in_range(&pwm_to_begin_motorB, -1,1);
Tess 2:83dd9068b6c5 183 motordirB.write(1);
Tess 3:c4df318913b8 184 pwm_motorB.write(abs(pwm_to_begin_motorB));
Tess 3:c4df318913b8 185 }
Tess 3:c4df318913b8 186 pwm_motorA.write(0);
Tess 3:c4df318913b8 187 pwm_motorB.write(0);
Tess 0:f492ec86159e 188
Tess 0:f492ec86159e 189
Tess 3:c4df318913b8 190
Tess 3:c4df318913b8 191 /*Create a ticker, and let it call the */
Tess 3:c4df318913b8 192 /*function 'setlooptimerflag' every 0.01s */
Tess 2:83dd9068b6c5 193
Tess 2:83dd9068b6c5 194 //INFINITE LOOP
Tess 3:c4df318913b8 195 while(1) {
Tess 3:c4df318913b8 196 /* Wait until looptimer flag is true. */
Tess 3:c4df318913b8 197 /* '!=' means not-is-equal */
Tess 3:c4df318913b8 198 while(looptimerflag != true);
Tess 3:c4df318913b8 199 /* Clear the looptimerflag, otherwise */
Tess 3:c4df318913b8 200 /* the program would simply continue */
Tess 3:c4df318913b8 201 /* without waitingin the next iteration*/
Tess 3:c4df318913b8 202 looptimerflag = false;
Tess 2:83dd9068b6c5 203
Tess 2:83dd9068b6c5 204
Tess 2:83dd9068b6c5 205
Tess 2:83dd9068b6c5 206
Tess 3:c4df318913b8 207 /* Read potmeter value, apply some math */
Tess 3:c4df318913b8 208 /* to get useful setpoint value */
Tess 3:c4df318913b8 209 setpointA = (potmeterA.read()-0.5)*(631/2); // bereik van 71 graden dit afhankelijk van waar nul punt zit en waar heel wil. Dus afh. van EMG lezen
Tess 3:c4df318913b8 210 setpointB = (potmeterB.read()-0.5)*(415/2); // bereik van 46.7 graden 1000 dan rotatie langzamer maken als lager maakt.
Tess 2:83dd9068b6c5 211
Tess 2:83dd9068b6c5 212
Tess 3:c4df318913b8 213 // motor A moet de hoek altijd binnen 53.4 tot en met 124.3 graden liggen
Tess 3:c4df318913b8 214 // motor B moet de hoek altijd binnen 30.2 tot en met -16.5 graden liggen
Tess 3:c4df318913b8 215 keep_in_range(&setpointA, 474, 1105);
Tess 3:c4df318913b8 216 keep_in_range(&setpointB, -147, 269);
Tess 2:83dd9068b6c5 217
Tess 3:c4df318913b8 218 /* Print setpoint and current position to serial terminal*/
Tess 3:c4df318913b8 219 pc.printf("s: %f, %d ", setpointA, motorA.getPosition());
Tess 3:c4df318913b8 220 pc.printf("s: %f, %d \n\r", setpointB, motorB.getPosition());
Tess 2:83dd9068b6c5 221
Tess 3:c4df318913b8 222 /* This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor */
Tess 3:c4df318913b8 223 pwm_to_motorA = (setpointA - motorA.getPosition())*.001;
Tess 3:c4df318913b8 224 pwm_to_motorB = (setpointB - motorB.getPosition())*.001;
Tess 3:c4df318913b8 225 /* Coerce pwm value if outside range */
Tess 3:c4df318913b8 226 /* Not strictly needed here, but useful */
Tess 3:c4df318913b8 227 /* if doing other calculations with pwm value */
Tess 3:c4df318913b8 228 keep_in_range(&pwm_to_motorA, -1,1);
Tess 3:c4df318913b8 229 keep_in_range(&pwm_to_motorB, -1,1);
Tess 2:83dd9068b6c5 230
Tess 3:c4df318913b8 231 /* Control the motor direction pin. based on */
Tess 3:c4df318913b8 232 /* the sign of your pwm value. If your */
Tess 3:c4df318913b8 233 /* motor keeps spinning when running this code */
Tess 3:c4df318913b8 234 /* you probably need to swap the motor wires, */
Tess 3:c4df318913b8 235 /* or swap the 'write(1)' and 'write(0)' below */
Tess 3:c4df318913b8 236 if(pwm_to_motorA > 0)
Tess 3:c4df318913b8 237 motordirA.write(1);
Tess 3:c4df318913b8 238 else
Tess 3:c4df318913b8 239 motordirA.write(0);
Tess 3:c4df318913b8 240 if(pwm_to_motorB > 0)
Tess 3:c4df318913b8 241 motordirB.write(1);
Tess 3:c4df318913b8 242 else
Tess 3:c4df318913b8 243 motordirB.write(0);
Tess 2:83dd9068b6c5 244
Tess 2:83dd9068b6c5 245
Tess 3:c4df318913b8 246 //WRITE VALUE TO MOTOR
Tess 3:c4df318913b8 247 /* Take the absolute value of the PWM to send */
Tess 3:c4df318913b8 248 /* to the motor. */
Tess 3:c4df318913b8 249 pwm_motorA.write(abs(pwm_to_motorA));
Tess 3:c4df318913b8 250 pwm_motorB.write(abs(pwm_to_motorB));
Tess 3:c4df318913b8 251 }
Tess 0:f492ec86159e 252 }
Tess 0:f492ec86159e 253
Tess 0:f492ec86159e 254
Tess 0:f492ec86159e 255 //coerces value 'in' to min or max when exceeding those values
Tess 0:f492ec86159e 256 //if you'd like to understand the statement below take a google for
Tess 0:f492ec86159e 257 //'ternary operators'.
Tess 0:f492ec86159e 258 void keep_in_range(float * in, float min, float max)
Tess 0:f492ec86159e 259 {
Tess 0:f492ec86159e 260 *in > min ? *in < max? : *in = max: *in = min;
Tess 0:f492ec86159e 261 }
Tess 0:f492ec86159e 262
Tess 0:f492ec86159e 263
Tess 0:f492ec86159e 264