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@3:c4df318913b8, 2013-10-30 (annotated)

Committer:
Tess
Date:
Wed Oct 30 08:41:50 2013 +0000
Revision:
3:c4df318913b8
Parent:
2:83dd9068b6c5
Child:
4:8344a3edd96c
gd

;

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 3:c4df318913b8 140 motorA.setPosition(0);
Tess 3:c4df318913b8 141 pwm_motorA.write(0);
Tess 2:83dd9068b6c5 142
Tess 3:c4df318913b8 143 Ticker looptimer2;
Tess 3:c4df318913b8 144 looptimer2.attach(setlooptimerflag,0.01);
Tess 2:83dd9068b6c5 145
Tess 3:c4df318913b8 146 while(pwm_to_begin_motorB >= 0) {
Tess 3:c4df318913b8 147 while(looptimerflag != true);
Tess 3:c4df318913b8 148 looptimerflag = false;
Tess 0:f492ec86159e 149
Tess 3:c4df318913b8 150 setpoint_beginB = 192; //192
Tess 3:c4df318913b8 151 pwm_to_begin_motorB = (setpoint_beginB - motorB.getPosition()) *.001;
Tess 3:c4df318913b8 152 keep_in_range(&pwm_to_begin_motorB, -1,1);
Tess 3:c4df318913b8 153 //if(pwm_to_begin_motorB <= 0) {
Tess 3:c4df318913b8 154 // motordirB.write(0);
Tess 3:c4df318913b8 155 //motorB.setPosition(0);
Tess 3:c4df318913b8 156 //pwm_motorB.write(0);
Tess 3:c4df318913b8 157 //} else {
Tess 3:c4df318913b8 158 motordirB.write(1);
Tess 3:c4df318913b8 159 pwm_motorB.write(abs(pwm_to_begin_motorB));
Tess 3:c4df318913b8 160 }
Tess 3:c4df318913b8 161 motorB.setPosition(0);
Tess 3:c4df318913b8 162 pwm_motorB.write(0);
Tess 2:83dd9068b6c5 163
Tess 2:83dd9068b6c5 164 // nu naar positie rechtsonderhoek A4 deze is motor A 531.6 en motor B 460
Tess 3:c4df318913b8 165
Tess 3:c4df318913b8 166 //while(0) ///1
Tess 3:c4df318913b8 167 //{
Tess 3:c4df318913b8 168 // while(looptimerflag != true);
Tess 3:c4df318913b8 169 //
Tess 3:c4df318913b8 170 // looptimerflag = false;
Tess 3:c4df318913b8 171
Tess 3:c4df318913b8 172 // setpoint_rechtsonderA = 531; // naar rechter onderhoek
Tess 3:c4df318913b8 173 // pwm_to_rechtsonder_motorA = setpoint_rechtsonderA *.001;
Tess 3:c4df318913b8 174 // keep_in_range(&pwm_to_rechtsonder_motorA, -1, 1 );
Tess 3:c4df318913b8 175 // if(pwm_to_rechtsonder_motorA > 0.531) {
Tess 3:c4df318913b8 176 // motordirA.write(0);
Tess 3:c4df318913b8 177 // pwm_motorA.write(0);
Tess 3:c4df318913b8 178 // } else
Tess 3:c4df318913b8 179 // motordirA.write(1);
Tess 3:c4df318913b8 180 // pwm_motorA.write(abs(pwm_to_rechtsonder_motorA));
Tess 3:c4df318913b8 181
Tess 3:c4df318913b8 182 // while(looptimerflag != true);
Tess 3:c4df318913b8 183 // looptimerflag = false;
Tess 2:83dd9068b6c5 184
Tess 3:c4df318913b8 185 // setpoint_rechtsonderB = 460;
Tess 3:c4df318913b8 186 // pwm_to_rechtsonder_motorB = setpoint_rechtsonderB *.001;
Tess 3:c4df318913b8 187 // keep_in_range(&pwm_to_rechtsonder_motorB, -1,1);
Tess 3:c4df318913b8 188 // if(pwm_to_rechtsonder_motorB > 0.460) {
Tess 3:c4df318913b8 189 // motordirB.write(0);
Tess 3:c4df318913b8 190 // pwm_motorB.write(0);
Tess 3:c4df318913b8 191 // } else
Tess 3:c4df318913b8 192 // motordirB.write(1);
Tess 3:c4df318913b8 193 // pwm_motorB.write(abs(pwm_to_rechtsonder_motorB));
Tess 3:c4df318913b8 194
Tess 3:c4df318913b8 195 //}
Tess 0:f492ec86159e 196
Tess 3:c4df318913b8 197 Ticker looptimer3;
Tess 3:c4df318913b8 198 looptimer3.attach(setlooptimerflag,0.01);
Tess 3:c4df318913b8 199
Tess 3:c4df318913b8 200 while((pwm_to_begin_motorA >= 0)&&(pwm_to_begin_motorB >= 0)) {
Tess 3:c4df318913b8 201 while(looptimerflag != true);
Tess 3:c4df318913b8 202
Tess 3:c4df318913b8 203 looptimerflag = false;
Tess 3:c4df318913b8 204
Tess 3:c4df318913b8 205 setpoint_beginA = 1000;//532 // naar rechteronderhoek A4
Tess 3:c4df318913b8 206 pwm_to_begin_motorA = (setpoint_beginA - motorA.getPosition()) *.001;
Tess 3:c4df318913b8 207 keep_in_range(&pwm_to_begin_motorA, -1, 1 );
Tess 3:c4df318913b8 208
Tess 2:83dd9068b6c5 209 motordirA.write(0);
Tess 3:c4df318913b8 210 pwm_motorA.write(abs(pwm_to_begin_motorA));
Tess 2:83dd9068b6c5 211
Tess 3:c4df318913b8 212 while(looptimerflag != true);
Tess 3:c4df318913b8 213
Tess 3:c4df318913b8 214 looptimerflag = false;
Tess 2:83dd9068b6c5 215
Tess 3:c4df318913b8 216 setpoint_beginB = 1000; //460
Tess 3:c4df318913b8 217 pwm_to_begin_motorB = (setpoint_beginB - motorB.getPosition()) *.001;
Tess 3:c4df318913b8 218 keep_in_range(&pwm_to_begin_motorB, -1,1);
Tess 2:83dd9068b6c5 219 motordirB.write(1);
Tess 3:c4df318913b8 220 pwm_motorB.write(abs(pwm_to_begin_motorB));
Tess 3:c4df318913b8 221 }
Tess 3:c4df318913b8 222 pwm_motorA.write(0);
Tess 3:c4df318913b8 223 pwm_motorB.write(0);
Tess 0:f492ec86159e 224
Tess 0:f492ec86159e 225
Tess 3:c4df318913b8 226
Tess 3:c4df318913b8 227 /*Create a ticker, and let it call the */
Tess 3:c4df318913b8 228 /*function 'setlooptimerflag' every 0.01s */
Tess 3:c4df318913b8 229 Ticker looptimer;
Tess 3:c4df318913b8 230 looptimer.attach(setlooptimerflag,0.01);
Tess 2:83dd9068b6c5 231
Tess 2:83dd9068b6c5 232 //INFINITE LOOP
Tess 3:c4df318913b8 233 while(1) {
Tess 3:c4df318913b8 234 /* Wait until looptimer flag is true. */
Tess 3:c4df318913b8 235 /* '!=' means not-is-equal */
Tess 3:c4df318913b8 236 while(looptimerflag != true);
Tess 3:c4df318913b8 237 /* Clear the looptimerflag, otherwise */
Tess 3:c4df318913b8 238 /* the program would simply continue */
Tess 3:c4df318913b8 239 /* without waitingin the next iteration*/
Tess 3:c4df318913b8 240 looptimerflag = false;
Tess 2:83dd9068b6c5 241
Tess 2:83dd9068b6c5 242
Tess 2:83dd9068b6c5 243
Tess 2:83dd9068b6c5 244
Tess 3:c4df318913b8 245 /* Read potmeter value, apply some math */
Tess 3:c4df318913b8 246 /* to get useful setpoint value */
Tess 3:c4df318913b8 247 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 248 setpointB = (potmeterB.read()-0.5)*(415/2); // bereik van 46.7 graden 1000 dan rotatie langzamer maken als lager maakt.
Tess 2:83dd9068b6c5 249
Tess 2:83dd9068b6c5 250
Tess 3:c4df318913b8 251 // motor A moet de hoek altijd binnen 53.4 tot en met 124.3 graden liggen
Tess 3:c4df318913b8 252 // motor B moet de hoek altijd binnen 30.2 tot en met -16.5 graden liggen
Tess 3:c4df318913b8 253 keep_in_range(&setpointA, 474, 1105);
Tess 3:c4df318913b8 254 keep_in_range(&setpointB, -147, 269);
Tess 2:83dd9068b6c5 255
Tess 3:c4df318913b8 256 /* Print setpoint and current position to serial terminal*/
Tess 3:c4df318913b8 257 pc.printf("s: %f, %d ", setpointA, motorA.getPosition());
Tess 3:c4df318913b8 258 pc.printf("s: %f, %d \n\r", setpointB, motorB.getPosition());
Tess 2:83dd9068b6c5 259
Tess 3:c4df318913b8 260 /* This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor */
Tess 3:c4df318913b8 261 pwm_to_motorA = (setpointA - motorA.getPosition())*.001;
Tess 3:c4df318913b8 262 pwm_to_motorB = (setpointB - motorB.getPosition())*.001;
Tess 3:c4df318913b8 263 /* Coerce pwm value if outside range */
Tess 3:c4df318913b8 264 /* Not strictly needed here, but useful */
Tess 3:c4df318913b8 265 /* if doing other calculations with pwm value */
Tess 3:c4df318913b8 266 keep_in_range(&pwm_to_motorA, -1,1);
Tess 3:c4df318913b8 267 keep_in_range(&pwm_to_motorB, -1,1);
Tess 2:83dd9068b6c5 268
Tess 3:c4df318913b8 269 /* Control the motor direction pin. based on */
Tess 3:c4df318913b8 270 /* the sign of your pwm value. If your */
Tess 3:c4df318913b8 271 /* motor keeps spinning when running this code */
Tess 3:c4df318913b8 272 /* you probably need to swap the motor wires, */
Tess 3:c4df318913b8 273 /* or swap the 'write(1)' and 'write(0)' below */
Tess 3:c4df318913b8 274 if(pwm_to_motorA > 0)
Tess 3:c4df318913b8 275 motordirA.write(1);
Tess 3:c4df318913b8 276 else
Tess 3:c4df318913b8 277 motordirA.write(0);
Tess 3:c4df318913b8 278 if(pwm_to_motorB > 0)
Tess 3:c4df318913b8 279 motordirB.write(1);
Tess 3:c4df318913b8 280 else
Tess 3:c4df318913b8 281 motordirB.write(0);
Tess 2:83dd9068b6c5 282
Tess 2:83dd9068b6c5 283
Tess 3:c4df318913b8 284 //WRITE VALUE TO MOTOR
Tess 3:c4df318913b8 285 /* Take the absolute value of the PWM to send */
Tess 3:c4df318913b8 286 /* to the motor. */
Tess 3:c4df318913b8 287 pwm_motorA.write(abs(pwm_to_motorA));
Tess 3:c4df318913b8 288 pwm_motorB.write(abs(pwm_to_motorB));
Tess 3:c4df318913b8 289 }
Tess 0:f492ec86159e 290 }
Tess 0:f492ec86159e 291
Tess 0:f492ec86159e 292
Tess 0:f492ec86159e 293 //coerces value 'in' to min or max when exceeding those values
Tess 0:f492ec86159e 294 //if you'd like to understand the statement below take a google for
Tess 0:f492ec86159e 295 //'ternary operators'.
Tess 0:f492ec86159e 296 void keep_in_range(float * in, float min, float max)
Tess 0:f492ec86159e 297 {
Tess 0:f492ec86159e 298 *in > min ? *in < max? : *in = max: *in = min;
Tess 0:f492ec86159e 299 }
Tess 0:f492ec86159e 300
Tess 0:f492ec86159e 301
Tess 0:f492ec86159e 302