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@2:83dd9068b6c5, 2013-10-29 (annotated)

Committer:
Tess
Date:
Tue Oct 29 16:02:31 2013 +0000
Revision:
2:83dd9068b6c5
Parent:
1:7cafc9042056
Child:
3:c4df318913b8
jfidj

;

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