Tess Groeneveld / Mbed 2 deprecated POTMETEREINDVERSIETEKENEN_WERKEND

Hierbij het programma waarmee de robot via potmeters kan worden bestuurd. Dit programma is ook gebruikt voor EMG aansturing aangezien het EMG op een andere bord wordt gefilterd en omgezet. Dit EMG programma is ook gepublished.

Dependencies:   Encoder MODSERIAL mbed

Fork of POTMETEREINDVERSIETEKENEN by Tess Groeneveld

main.cpp@0:73e1c3fd28b5, 2013-11-05 (annotated)

Committer:
Tess
Date:
Tue Nov 05 09:35:05 2013 +0000
Revision:
0:73e1c3fd28b5
Eindversie Potmeter gestuurde tekenrobot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Tess 0:73e1c3fd28b5 1 #include "mbed.h"
Tess 0:73e1c3fd28b5 2 #include "encoder.h"
Tess 0:73e1c3fd28b5 3 #include "MODSERIAL.h"
Tess 0:73e1c3fd28b5 4
Tess 0:73e1c3fd28b5 5 /*******************************************************************************
Tess 0:73e1c3fd28b5 6 * *
Tess 0:73e1c3fd28b5 7 * Code can be found at http://mbed.org/users/vsluiter/code/BMT-K9-Regelaar/ *
Tess 0:73e1c3fd28b5 8 * *
Tess 0:73e1c3fd28b5 9 ********************************************************************************/
Tess 0:73e1c3fd28b5 10
Tess 0:73e1c3fd28b5 11 /** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/
Tess 0:73e1c3fd28b5 12 void keep_in_range(float * in, float min, float max);
Tess 0:73e1c3fd28b5 13
Tess 0:73e1c3fd28b5 14 /** variable to show when a new loop can be started*/
Tess 0:73e1c3fd28b5 15 /** volatile means that it can be changed in an */
Tess 0:73e1c3fd28b5 16 /** interrupt routine, and that that change is vis-*/
Tess 0:73e1c3fd28b5 17 /** ible in the main loop. */
Tess 0:73e1c3fd28b5 18
Tess 0:73e1c3fd28b5 19 volatile bool looptimerflag;
Tess 0:73e1c3fd28b5 20
Tess 0:73e1c3fd28b5 21 /** function called by Ticker "looptimer" */
Tess 0:73e1c3fd28b5 22 /** variable 'looptimerflag' is set to 'true' */
Tess 0:73e1c3fd28b5 23 /** each time the looptimer expires. */
Tess 0:73e1c3fd28b5 24 void setlooptimerflag(void)
Tess 0:73e1c3fd28b5 25 {
Tess 0:73e1c3fd28b5 26 looptimerflag = true;
Tess 0:73e1c3fd28b5 27 }
Tess 0:73e1c3fd28b5 28
Tess 0:73e1c3fd28b5 29
Tess 0:73e1c3fd28b5 30 int main()
Tess 0:73e1c3fd28b5 31 {
Tess 0:73e1c3fd28b5 32 //LOCAL VARIABLES
Tess 0:73e1c3fd28b5 33 /*Potmeter input*/
Tess 0:73e1c3fd28b5 34 AnalogIn potmeterA(PTC2);
Tess 0:73e1c3fd28b5 35 AnalogIn potmeterB(PTB2);
Tess 0:73e1c3fd28b5 36 /* Encoder, using my encoder library */
Tess 0:73e1c3fd28b5 37 /* First pin should be PTDx or PTAx */
Tess 0:73e1c3fd28b5 38 /* because those pins can be used as */
Tess 0:73e1c3fd28b5 39 /* InterruptIn */
Tess 0:73e1c3fd28b5 40 Encoder motorA(PTD4,PTC8);
Tess 0:73e1c3fd28b5 41 Encoder motorB(PTD0,PTD2);
Tess 0:73e1c3fd28b5 42 /* MODSERIAL to get non-blocking Serial*/
Tess 0:73e1c3fd28b5 43 MODSERIAL pc(USBTX,USBRX);
Tess 0:73e1c3fd28b5 44 /* PWM control to motor */
Tess 0:73e1c3fd28b5 45 PwmOut pwm_motorA(PTA12);
Tess 0:73e1c3fd28b5 46 PwmOut pwm_motorB(PTA5);
Tess 0:73e1c3fd28b5 47 /* Direction pin */
Tess 0:73e1c3fd28b5 48 DigitalOut motordirA(PTD3);
Tess 0:73e1c3fd28b5 49 DigitalOut motordirB(PTD1);
Tess 0:73e1c3fd28b5 50 /* variable to store setpoint in */
Tess 0:73e1c3fd28b5 51 float setpointA;
Tess 0:73e1c3fd28b5 52 float setpointB;
Tess 0:73e1c3fd28b5 53 /* variable to store pwm value in*/
Tess 0:73e1c3fd28b5 54 float pwm_to_motorA;
Tess 0:73e1c3fd28b5 55 float pwm_to_motorB;
Tess 0:73e1c3fd28b5 56
Tess 0:73e1c3fd28b5 57 /* variable to store setpoint in */
Tess 0:73e1c3fd28b5 58 float setpoint_beginA;
Tess 0:73e1c3fd28b5 59 float setpoint_beginB;
Tess 0:73e1c3fd28b5 60 float setpoint_rechtsonderA;
Tess 0:73e1c3fd28b5 61 float setpoint_rechtsonderB;
Tess 0:73e1c3fd28b5 62
Tess 0:73e1c3fd28b5 63 /* variable to store pwm value in*/
Tess 0:73e1c3fd28b5 64
Tess 0:73e1c3fd28b5 65 float pwm_to_begin_motorA = 0;
Tess 0:73e1c3fd28b5 66 float pwm_to_begin_motorB = 0;
Tess 0:73e1c3fd28b5 67
Tess 0:73e1c3fd28b5 68 float pwm_to_rechtsonder_motorA;
Tess 0:73e1c3fd28b5 69 float pwm_to_rechtsonder_motorB;
Tess 0:73e1c3fd28b5 70
Tess 0:73e1c3fd28b5 71 /* variable to store positions in*/
Tess 0:73e1c3fd28b5 72 int32_t positionmotorA_t0;
Tess 0:73e1c3fd28b5 73 int32_t positionmotorB_t0;
Tess 0:73e1c3fd28b5 74 int32_t positionmotorA_t_1;
Tess 0:73e1c3fd28b5 75 int32_t positionmotorB_t_1;
Tess 0:73e1c3fd28b5 76 int32_t positiondifference_motorA;
Tess 0:73e1c3fd28b5 77 int32_t positiondifference_motorB;
Tess 0:73e1c3fd28b5 78
Tess 0:73e1c3fd28b5 79
Tess 0:73e1c3fd28b5 80
Tess 0:73e1c3fd28b5 81 //START OF CODE
Tess 0:73e1c3fd28b5 82
Tess 0:73e1c3fd28b5 83 /*Set the baudrate (use this number in RealTerm too! */
Tess 0:73e1c3fd28b5 84 pc.baud(921600);
Tess 0:73e1c3fd28b5 85
Tess 0:73e1c3fd28b5 86 motordirB.write(0);
Tess 0:73e1c3fd28b5 87 pwm_motorB.write(.1); //0.08
Tess 0:73e1c3fd28b5 88 positionmotorB_t0 = motorB.getPosition();
Tess 0:73e1c3fd28b5 89 do {
Tess 0:73e1c3fd28b5 90 wait(0.2);
Tess 0:73e1c3fd28b5 91 positionmotorB_t_1 = positionmotorB_t0 ;
Tess 0:73e1c3fd28b5 92 positionmotorB_t0 = motorB.getPosition();
Tess 0:73e1c3fd28b5 93 positiondifference_motorB = abs(positionmotorB_t0 - positionmotorB_t_1);
Tess 0:73e1c3fd28b5 94 } while(positiondifference_motorB > 10);
Tess 0:73e1c3fd28b5 95 motorB.setPosition(0);
Tess 0:73e1c3fd28b5 96 pwm_motorB.write(0);
Tess 0:73e1c3fd28b5 97
Tess 0:73e1c3fd28b5 98 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 0:73e1c3fd28b5 99
Tess 0:73e1c3fd28b5 100 motordirA.write(1);
Tess 0:73e1c3fd28b5 101 pwm_motorA.write(.1);
Tess 0:73e1c3fd28b5 102 positionmotorA_t0 = motorA.getPosition();
Tess 0:73e1c3fd28b5 103 do {
Tess 0:73e1c3fd28b5 104 wait(0.2);
Tess 0:73e1c3fd28b5 105 positionmotorA_t_1 = positionmotorA_t0 ;
Tess 0:73e1c3fd28b5 106 positionmotorA_t0 = motorA.getPosition();
Tess 0:73e1c3fd28b5 107 positiondifference_motorA = abs(positionmotorA_t0 - positionmotorA_t_1);
Tess 0:73e1c3fd28b5 108 } while(positiondifference_motorA > 10);
Tess 0:73e1c3fd28b5 109 motorA.setPosition(0);
Tess 0:73e1c3fd28b5 110 pwm_motorA.write(0);
Tess 0:73e1c3fd28b5 111
Tess 0:73e1c3fd28b5 112 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 0:73e1c3fd28b5 113
Tess 0:73e1c3fd28b5 114 // 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 0:73e1c3fd28b5 115
Tess 0:73e1c3fd28b5 116 motordirA.write(0);
Tess 0:73e1c3fd28b5 117 pwm_motorA.write(.08);
Tess 0:73e1c3fd28b5 118 do {
Tess 0:73e1c3fd28b5 119 setpoint_beginA = -63; // x-as
Tess 0:73e1c3fd28b5 120 pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001); // + omdat men met een negatieve hoekverdraaiing werkt.
Tess 0:73e1c3fd28b5 121 wait(0.2);
Tess 0:73e1c3fd28b5 122 keep_in_range(&pwm_to_begin_motorA, -1, 1 );
Tess 0:73e1c3fd28b5 123 motordirA.write(0);
Tess 0:73e1c3fd28b5 124 pwm_motorA.write(pwm_to_begin_motorA);
Tess 0:73e1c3fd28b5 125 } while(pwm_to_begin_motorA <= 0);
Tess 0:73e1c3fd28b5 126 motorA.setPosition(0);
Tess 0:73e1c3fd28b5 127 pwm_motorA.write(0);
Tess 0:73e1c3fd28b5 128
Tess 0:73e1c3fd28b5 129 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 0:73e1c3fd28b5 130
Tess 0:73e1c3fd28b5 131 // hierna moet motor A naar de rechtsonder A4. Motor A 532.
Tess 0:73e1c3fd28b5 132
Tess 0:73e1c3fd28b5 133 motordirA.write(0);
Tess 0:73e1c3fd28b5 134 pwm_motorA.write(0.08);
Tess 0:73e1c3fd28b5 135 do {
Tess 0:73e1c3fd28b5 136 setpoint_rechtsonderA = -532; // -532 rechtsonder positie A4
Tess 0:73e1c3fd28b5 137 pwm_to_rechtsonder_motorA = abs((setpoint_rechtsonderA + motorA.getPosition()) *.001);
Tess 0:73e1c3fd28b5 138 wait(0.2);
Tess 0:73e1c3fd28b5 139 keep_in_range(&pwm_to_rechtsonder_motorA, -1, 1 );
Tess 0:73e1c3fd28b5 140 motordirA.write(0);
Tess 0:73e1c3fd28b5 141 pwm_motorA.write(pwm_to_rechtsonder_motorA);
Tess 0:73e1c3fd28b5 142 } while(pwm_to_rechtsonder_motorA <= 0);
Tess 0:73e1c3fd28b5 143 pwm_motorA.write(0);
Tess 0:73e1c3fd28b5 144
Tess 0:73e1c3fd28b5 145 wait(1);
Tess 0:73e1c3fd28b5 146
Tess 0:73e1c3fd28b5 147 // Hierna moet motor B 21.6 (192) graden naar links om naar x-as te gaan.
Tess 0:73e1c3fd28b5 148
Tess 0:73e1c3fd28b5 149 motordirB.write(1);
Tess 0:73e1c3fd28b5 150 pwm_motorB.write(.08);
Tess 0:73e1c3fd28b5 151 do {
Tess 0:73e1c3fd28b5 152 setpoint_beginB = 192; // x-as
Tess 0:73e1c3fd28b5 153 pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
Tess 0:73e1c3fd28b5 154 wait(0.2);
Tess 0:73e1c3fd28b5 155 keep_in_range(&pwm_to_begin_motorB, -1, 1 );
Tess 0:73e1c3fd28b5 156 motordirB.write(1);
Tess 0:73e1c3fd28b5 157 pwm_motorB.write(pwm_to_begin_motorB);
Tess 0:73e1c3fd28b5 158 } while(pwm_to_begin_motorB <= 0);
Tess 0:73e1c3fd28b5 159 motorB.setPosition(0);
Tess 0:73e1c3fd28b5 160 pwm_motorB.write(0);
Tess 0:73e1c3fd28b5 161
Tess 0:73e1c3fd28b5 162 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 0:73e1c3fd28b5 163
Tess 0:73e1c3fd28b5 164 // Hierna moet motor B van x-as naar de rechtsonder A4 positie. Motor B 460.
Tess 0:73e1c3fd28b5 165
Tess 0:73e1c3fd28b5 166 motordirB.write(1);
Tess 0:73e1c3fd28b5 167 pwm_motorB.write(0.08);
Tess 0:73e1c3fd28b5 168 do {
Tess 0:73e1c3fd28b5 169 setpoint_rechtsonderB = 460; // rechtsonder positie A4
Tess 0:73e1c3fd28b5 170 pwm_to_rechtsonder_motorB = abs((setpoint_rechtsonderB - motorB.getPosition()) *.001);
Tess 0:73e1c3fd28b5 171 wait(0.2);
Tess 0:73e1c3fd28b5 172 keep_in_range(&pwm_to_rechtsonder_motorB, -1, 1 );
Tess 0:73e1c3fd28b5 173 motordirB.write(1);
Tess 0:73e1c3fd28b5 174 pwm_motorB.write(pwm_to_rechtsonder_motorB);
Tess 0:73e1c3fd28b5 175 } while(pwm_to_rechtsonder_motorB <= 0);
Tess 0:73e1c3fd28b5 176 pwm_motorB.write(0);
Tess 0:73e1c3fd28b5 177
Tess 0:73e1c3fd28b5 178 wait(1);
Tess 0:73e1c3fd28b5 179
Tess 0:73e1c3fd28b5 180 /*Create a ticker, and let it call the */
Tess 0:73e1c3fd28b5 181 /*function 'setlooptimerflag' every 0.01s */
Tess 0:73e1c3fd28b5 182 Ticker looptimer;
Tess 0:73e1c3fd28b5 183 looptimer.attach(setlooptimerflag,0.01);
Tess 0:73e1c3fd28b5 184
Tess 0:73e1c3fd28b5 185 //INFINITE LOOP
Tess 0:73e1c3fd28b5 186 while(1) {
Tess 0:73e1c3fd28b5 187 /* Wait until looptimer flag is true. */
Tess 0:73e1c3fd28b5 188 /* '!=' means not-is-equal */
Tess 0:73e1c3fd28b5 189 while(looptimerflag != true);
Tess 0:73e1c3fd28b5 190 /* Clear the looptimerflag, otherwise */
Tess 0:73e1c3fd28b5 191 /* the program would simply continue */
Tess 0:73e1c3fd28b5 192 /* without waitingin the next iteration*/
Tess 0:73e1c3fd28b5 193 looptimerflag = false;
Tess 0:73e1c3fd28b5 194
Tess 0:73e1c3fd28b5 195 /* Read potmeter value, apply some math */
Tess 0:73e1c3fd28b5 196 /* to get useful setpoint value */
Tess 0:73e1c3fd28b5 197 setpointA = (potmeterA.read()-1)*(1000); //631/2
Tess 0:73e1c3fd28b5 198 setpointB = (potmeterB.read()-0.5)*(1000); //871 dan rotatie langzamer maken als lager maakt.
Tess 0:73e1c3fd28b5 199
Tess 0:73e1c3fd28b5 200 /* Print setpoint and current position to serial terminal*/
Tess 0:73e1c3fd28b5 201 pc.printf("s: %f, %d ", setpointA, motorA.getPosition());
Tess 0:73e1c3fd28b5 202 pc.printf("s: %f, %d \n\r", setpointB, motorB.getPosition());
Tess 0:73e1c3fd28b5 203
Tess 0:73e1c3fd28b5 204 /* This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor */
Tess 0:73e1c3fd28b5 205 pwm_to_motorA = (setpointA - motorA.getPosition())*.001;
Tess 0:73e1c3fd28b5 206 pwm_to_motorB = (setpointB - motorB.getPosition())*.001;
Tess 0:73e1c3fd28b5 207
Tess 0:73e1c3fd28b5 208 /* Coerce pwm value if outside range */
Tess 0:73e1c3fd28b5 209 /* Not strictly needed here, but useful */
Tess 0:73e1c3fd28b5 210 /* if doing other calculations with pwm value */
Tess 0:73e1c3fd28b5 211 keep_in_range(&pwm_to_motorA, -0.1,0.1);
Tess 0:73e1c3fd28b5 212 keep_in_range(&pwm_to_motorB, -0.1,0.1);
Tess 0:73e1c3fd28b5 213
Tess 0:73e1c3fd28b5 214 /* Control the motor direction pin. based on */
Tess 0:73e1c3fd28b5 215 /* the sign of your pwm value. If your */
Tess 0:73e1c3fd28b5 216 /* motor keeps spinning when running this code */
Tess 0:73e1c3fd28b5 217 /* you probably need to swap the motor wires, */
Tess 0:73e1c3fd28b5 218 /* or swap the 'write(1)' and 'write(0)' below */
Tess 0:73e1c3fd28b5 219 if(pwm_to_motorA > 0)
Tess 0:73e1c3fd28b5 220 motordirA.write(1);
Tess 0:73e1c3fd28b5 221 else
Tess 0:73e1c3fd28b5 222 motordirA.write(0);
Tess 0:73e1c3fd28b5 223 if(pwm_to_motorB > 0)
Tess 0:73e1c3fd28b5 224 motordirB.write(1);
Tess 0:73e1c3fd28b5 225 else
Tess 0:73e1c3fd28b5 226 motordirB.write(0);
Tess 0:73e1c3fd28b5 227
Tess 0:73e1c3fd28b5 228
Tess 0:73e1c3fd28b5 229 //WRITE VALUE TO MOTOR
Tess 0:73e1c3fd28b5 230 /* Take the absolute value of the PWM to send */
Tess 0:73e1c3fd28b5 231 /* to the motor. */
Tess 0:73e1c3fd28b5 232 pwm_motorA.write(abs(pwm_to_motorA));
Tess 0:73e1c3fd28b5 233 pwm_motorB.write(abs(pwm_to_motorB));
Tess 0:73e1c3fd28b5 234 }
Tess 0:73e1c3fd28b5 235 }
Tess 0:73e1c3fd28b5 236
Tess 0:73e1c3fd28b5 237
Tess 0:73e1c3fd28b5 238 //coerces value 'in' to min or max when exceeding those values
Tess 0:73e1c3fd28b5 239 //if you'd like to understand the statement below take a google for
Tess 0:73e1c3fd28b5 240 //'ternary operators'.
Tess 0:73e1c3fd28b5 241 void keep_in_range(float * in, float min, float max)
Tess 0:73e1c3fd28b5 242 {
Tess 0:73e1c3fd28b5 243 *in > min ? *in < max? : *in = max: *in = min;
Tess 0:73e1c3fd28b5 244 }