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@8:62e968f78878, 2013-10-30 (annotated)

Committer:
Tess
Date:
Wed Oct 30 13:16:12 2013 +0000
Revision:
8:62e968f78878
Parent:
7:1f88215b504c
Child:
9:c49363372755
The best version!! It works and now EMG is needed

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 7:1f88215b504c 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 7:1f88215b504c 76 // motor B zit onder en motor A zit boven en dus op zijn kop (en dus setpoint moet - zijn).
Tess 7:1f88215b504c 77
Tess 7:1f88215b504c 78 motordirB.write(0);
Tess 2:83dd9068b6c5 79 pwm_motorB.write(.08);
Tess 1:7cafc9042056 80 positionmotorB_t0 = motorB.getPosition();
Tess 1:7cafc9042056 81 do {
Tess 2:83dd9068b6c5 82 wait(0.2);
Tess 7:1f88215b504c 83 positionmotorB_t_1 = positionmotorB_t0 ;
Tess 1:7cafc9042056 84 positionmotorB_t0 = motorB.getPosition();
Tess 2:83dd9068b6c5 85 positiondifference_motorB = abs(positionmotorB_t0 - positionmotorB_t_1);
Tess 2:83dd9068b6c5 86 } while(positiondifference_motorB > 10);
Tess 2:83dd9068b6c5 87 motorB.setPosition(0);
Tess 2:83dd9068b6c5 88 pwm_motorB.write(0);
Tess 1:7cafc9042056 89
Tess 7:1f88215b504c 90 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 7:1f88215b504c 91
Tess 2:83dd9068b6c5 92 motordirA.write(1);
Tess 2:83dd9068b6c5 93 pwm_motorA.write(.08);
Tess 2:83dd9068b6c5 94 positionmotorA_t0 = motorA.getPosition();
Tess 2:83dd9068b6c5 95 do {
Tess 2:83dd9068b6c5 96 wait(0.2);
Tess 2:83dd9068b6c5 97 positionmotorA_t_1 = positionmotorA_t0 ;
Tess 2:83dd9068b6c5 98 positionmotorA_t0 = motorA.getPosition();
Tess 2:83dd9068b6c5 99 positiondifference_motorA = abs(positionmotorA_t0 - positionmotorA_t_1);
Tess 2:83dd9068b6c5 100 } while(positiondifference_motorA > 10);
Tess 2:83dd9068b6c5 101 motorA.setPosition(0);
Tess 2:83dd9068b6c5 102 pwm_motorA.write(0);
Tess 2:83dd9068b6c5 103
Tess 7:1f88215b504c 104 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 7:1f88215b504c 105
Tess 7:1f88215b504c 106 // 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 5:512dd4044486 107 // Hierna moet motor B 21.6 (192) graden naar links. Dus eerst motor A en dan motor B.
Tess 0:f492ec86159e 108
Tess 7:1f88215b504c 109 motordirA.write(0);
Tess 7:1f88215b504c 110 pwm_motorA.write(.08);
Tess 5:512dd4044486 111 do {
Tess 7:1f88215b504c 112 setpoint_beginA = -63;
Tess 7:1f88215b504c 113 pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001); // + omdat men met een negatieve hoekverdraaiing werkt.
Tess 8:62e968f78878 114 wait(0.2);
Tess 2:83dd9068b6c5 115 keep_in_range(&pwm_to_begin_motorA, -1, 1 );
Tess 2:83dd9068b6c5 116 motordirA.write(0);
Tess 5:512dd4044486 117 pwm_motorA.write(pwm_to_begin_motorA);
Tess 8:62e968f78878 118 } while(pwm_to_begin_motorA <= 0);
Tess 3:c4df318913b8 119 motorA.setPosition(0);
Tess 3:c4df318913b8 120 pwm_motorA.write(0);
Tess 2:83dd9068b6c5 121
Tess 7:1f88215b504c 122 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 7:1f88215b504c 123
Tess 7:1f88215b504c 124 motordirB.write(1);
Tess 7:1f88215b504c 125 pwm_motorB.write(.08);
Tess 7:1f88215b504c 126 do {
Tess 7:1f88215b504c 127 setpoint_beginB = 192;
Tess 7:1f88215b504c 128 pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
Tess 8:62e968f78878 129 wait(0.2);
Tess 7:1f88215b504c 130 keep_in_range(&pwm_to_begin_motorB, -1, 1 );
Tess 3:c4df318913b8 131 motordirB.write(1);
Tess 5:512dd4044486 132 pwm_motorB.write(pwm_to_begin_motorB);
Tess 8:62e968f78878 133 } while(pwm_to_begin_motorB <= 0);
Tess 3:c4df318913b8 134 motorB.setPosition(0);
Tess 3:c4df318913b8 135 pwm_motorB.write(0);
Tess 2:83dd9068b6c5 136
Tess 7:1f88215b504c 137 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 3:c4df318913b8 138
Tess 7:1f88215b504c 139 // Hierna willen we de motor van zijn x-as naar de rechtsonder positie van A4 te krijgen. Volgorde van motoren maakt nu niet uit.
Tess 7:1f88215b504c 140 // nu naar positie rechtsonderhoek A4 deze is voor motor A 532 en voor motor B 460
Tess 3:c4df318913b8 141
Tess 7:1f88215b504c 142 motordirB.write(1);
Tess 7:1f88215b504c 143 motordirA.write(0);
Tess 7:1f88215b504c 144 pwm_motorB.write(0.08);
Tess 7:1f88215b504c 145 pwm_motorA.write(0.08);
Tess 7:1f88215b504c 146 do {
Tess 8:62e968f78878 147 setpoint_beginA = -532;
Tess 7:1f88215b504c 148 pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001);
Tess 8:62e968f78878 149 setpoint_beginB = 460;
Tess 7:1f88215b504c 150 pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
Tess 8:62e968f78878 151 wait(0.2);
Tess 7:1f88215b504c 152 keep_in_range(&pwm_to_begin_motorB, -1, 1 );
Tess 7:1f88215b504c 153 motordirB.write(1);
Tess 7:1f88215b504c 154 pwm_motorB.write(pwm_to_begin_motorB);
Tess 3:c4df318913b8 155 keep_in_range(&pwm_to_begin_motorA, -1, 1 );
Tess 2:83dd9068b6c5 156 motordirA.write(0);
Tess 7:1f88215b504c 157 pwm_motorA.write(pwm_to_begin_motorA);
Tess 8:62e968f78878 158 } while((pwm_to_begin_motorA <= 0)&&(pwm_to_begin_motorB <= 0));
Tess 3:c4df318913b8 159 pwm_motorA.write(0);
Tess 3:c4df318913b8 160 pwm_motorB.write(0);
Tess 0:f492ec86159e 161
Tess 8:62e968f78878 162 // Nu zijn de motoren gekalibreed en staan ze op de startpositie.
Tess 8:62e968f78878 163 // Hierna het script dat EMG wordt omgezet in een positie verandering
Tess 8:62e968f78878 164
Tess 8:62e968f78878 165 /*Create a ticker, and let it call the */
Tess 8:62e968f78878 166 /*function 'setlooptimerflag' every 0.01s */
Tess 8:62e968f78878 167 Ticker looptimer;
Tess 8:62e968f78878 168 looptimer.attach(setlooptimerflag,0.01);
Tess 0:f492ec86159e 169
Tess 8:62e968f78878 170 //INFINITE LOOP
Tess 8:62e968f78878 171 while(1) {
Tess 8:62e968f78878 172
Tess 8:62e968f78878 173 while(looptimerflag != true);
Tess 8:62e968f78878 174 looptimerflag = false;
Tess 8:62e968f78878 175
Tess 8:62e968f78878 176 // hier EMG
Tess 8:62e968f78878 177 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 8:62e968f78878 178 setpointB = (potmeterB.read())*(415); // bereik van 46.7 graden
Tess 8:62e968f78878 179 pc.printf("s: %f, %d ", setpointA, motorA.getPosition());
Tess 8:62e968f78878 180
Tess 8:62e968f78878 181 // motor A moet de hoek altijd binnen 53.4 tot en met 124.3 graden liggen
Tess 8:62e968f78878 182 // motor B moet de hoek altijd binnen 30.2 tot en met -16.5 graden liggen
Tess 8:62e968f78878 183 keep_in_range(&setpointA, -1105, -474); // voor motor moet bereik zijn -1105 tot -474
Tess 8:62e968f78878 184 keep_in_range(&setpointB, -147, 269); // voor motor moet bereik zijn -147 tot 269
Tess 8:62e968f78878 185
Tess 8:62e968f78878 186 /* This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor */
Tess 8:62e968f78878 187 pwm_to_motorA = (setpointA - motorA.getPosition())*.001;
Tess 8:62e968f78878 188 pwm_to_motorB = (setpointB - motorB.getPosition())*.001;
Tess 8:62e968f78878 189
Tess 8:62e968f78878 190 keep_in_range(&pwm_to_motorA, -1,1);
Tess 8:62e968f78878 191 keep_in_range(&pwm_to_motorB, -1,1);
Tess 8:62e968f78878 192
Tess 8:62e968f78878 193 if(pwm_to_motorA > 0)
Tess 8:62e968f78878 194 motordirA.write(1);
Tess 8:62e968f78878 195 else
Tess 8:62e968f78878 196 motordirA.write(0);
Tess 8:62e968f78878 197 if(pwm_to_motorB > 0)
Tess 8:62e968f78878 198 motordirB.write(1);
Tess 8:62e968f78878 199 else
Tess 8:62e968f78878 200 motordirB.write(0);
Tess 8:62e968f78878 201
Tess 8:62e968f78878 202 pwm_motorA.write(abs(pwm_to_motorA));
Tess 8:62e968f78878 203 pwm_motorB.write(abs(pwm_to_motorB));
Tess 8:62e968f78878 204 }
Tess 0:f492ec86159e 205 }
Tess 0:f492ec86159e 206
Tess 0:f492ec86159e 207
Tess 0:f492ec86159e 208 void keep_in_range(float * in, float min, float max)
Tess 0:f492ec86159e 209 {
Tess 0:f492ec86159e 210 *in > min ? *in < max? : *in = max: *in = min;
Tess 0:f492ec86159e 211 }
Tess 0:f492ec86159e 212
Tess 0:f492ec86159e 213
Tess 0:f492ec86159e 214
Tess 7:1f88215b504c 215
Tess 7:1f88215b504c 216
Tess 7:1f88215b504c 217
Tess 7:1f88215b504c 218