Tess Groeneveld / Mbed 2 deprecated PODBACKUP

Aansturing motor met POT, niet werkend

Dependencies:   Encoder MODSERIAL mbed

main.cpp@0:6ca6892a17d0, 2013-11-04 (annotated)

Committer:
Tess
Date:
Mon Nov 04 23:09:57 2013 +0000
Revision:
0:6ca6892a17d0
Aansturing motor met POT, niet werkend

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Tess 0:6ca6892a17d0 1 #include "mbed.h"
Tess 0:6ca6892a17d0 2 #include "encoder.h"
Tess 0:6ca6892a17d0 3 #include "MODSERIAL.h"
Tess 0:6ca6892a17d0 4
Tess 0:6ca6892a17d0 5 /*******************************************************************************
Tess 0:6ca6892a17d0 6 * *
Tess 0:6ca6892a17d0 7 * Code can be found at http://mbed.org/users/vsluiter/code/BMT-K9-Regelaar/ *
Tess 0:6ca6892a17d0 8 * *
Tess 0:6ca6892a17d0 9 ********************************************************************************/
Tess 0:6ca6892a17d0 10
Tess 0:6ca6892a17d0 11 /** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/
Tess 0:6ca6892a17d0 12 void keep_in_range(float * in, float min, float max);
Tess 0:6ca6892a17d0 13
Tess 0:6ca6892a17d0 14 /** variable to show when a new loop can be started */
Tess 0:6ca6892a17d0 15 /** volatile means that it can be changed in an interrupt routine, and that that change is visible in the main loop. */
Tess 0:6ca6892a17d0 16
Tess 0:6ca6892a17d0 17 volatile bool looptimerflag;
Tess 0:6ca6892a17d0 18
Tess 0:6ca6892a17d0 19 /** function called by Ticker "looptimer" */
Tess 0:6ca6892a17d0 20 /** variable 'looptimerflag' is set to 'true' each time the looptimer expires.*/
Tess 0:6ca6892a17d0 21 void setlooptimerflag(void)
Tess 0:6ca6892a17d0 22 {
Tess 0:6ca6892a17d0 23 looptimerflag = true;
Tess 0:6ca6892a17d0 24 }
Tess 0:6ca6892a17d0 25
Tess 0:6ca6892a17d0 26 int main()
Tess 0:6ca6892a17d0 27 {
Tess 0:6ca6892a17d0 28 //LOCAL VARIABLES
Tess 0:6ca6892a17d0 29 AnalogIn potmeterA(PTC2);
Tess 0:6ca6892a17d0 30 AnalogIn potmeterB(PTB2);
Tess 0:6ca6892a17d0 31 Encoder motorA(PTD4,PTC8);
Tess 0:6ca6892a17d0 32 Encoder motorB(PTD0,PTD2);
Tess 0:6ca6892a17d0 33 MODSERIAL pc(USBTX,USBRX); // MODSERIAL to get non-blocking Serial
Tess 0:6ca6892a17d0 34 PwmOut pwm_motorA(PTA12); // PWM control to motor
Tess 0:6ca6892a17d0 35 PwmOut pwm_motorB(PTA5); // PWM control to motor
Tess 0:6ca6892a17d0 36 DigitalOut motordirA(PTD3); // Direction pin
Tess 0:6ca6892a17d0 37 DigitalOut motordirB(PTD1); // Direction pin
Tess 0:6ca6892a17d0 38
Tess 0:6ca6892a17d0 39 /* variable to store setpoint in */
Tess 0:6ca6892a17d0 40 float setpointA;
Tess 0:6ca6892a17d0 41 float setpointB;
Tess 0:6ca6892a17d0 42 float setpoint_beginA;
Tess 0:6ca6892a17d0 43 float setpoint_beginB;
Tess 0:6ca6892a17d0 44 float setpoint_rechtsonderA;
Tess 0:6ca6892a17d0 45 float setpoint_rechtsonderB;
Tess 0:6ca6892a17d0 46
Tess 0:6ca6892a17d0 47 /* variable to store pwm value in*/
Tess 0:6ca6892a17d0 48 float pwm_to_motorA;
Tess 0:6ca6892a17d0 49 float pwm_to_begin_motorA = 0;
Tess 0:6ca6892a17d0 50 float pwm_to_begin_motorB = 0;
Tess 0:6ca6892a17d0 51 float pwm_to_motorB;
Tess 0:6ca6892a17d0 52 float pwm_to_rechtsonder_motorA;
Tess 0:6ca6892a17d0 53 float pwm_to_rechtsonder_motorB;
Tess 0:6ca6892a17d0 54
Tess 0:6ca6892a17d0 55 /* variable for PD controller*/
Tess 0:6ca6892a17d0 56 const float dt = 0.002;
Tess 0:6ca6892a17d0 57 float Kp = 0.001; //0.0208
Tess 0:6ca6892a17d0 58 float Kd = 0.00004342; //0.0006897
Tess 0:6ca6892a17d0 59 float error_t0_A = 0;
Tess 0:6ca6892a17d0 60 float error_t0_B = 0;
Tess 0:6ca6892a17d0 61 float error_ti_A;
Tess 0:6ca6892a17d0 62 float error_ti_B;
Tess 0:6ca6892a17d0 63 float P_regelaar_A;
Tess 0:6ca6892a17d0 64 float P_regelaar_B;
Tess 0:6ca6892a17d0 65 float D_regelaar_A;
Tess 0:6ca6892a17d0 66 float D_regelaar_B;
Tess 0:6ca6892a17d0 67 float output_regelaar_A;
Tess 0:6ca6892a17d0 68 float output_regelaar_B;
Tess 0:6ca6892a17d0 69
Tess 0:6ca6892a17d0 70 /* variable to store positions in*/
Tess 0:6ca6892a17d0 71 int32_t positionmotorA_t0;
Tess 0:6ca6892a17d0 72 int32_t positionmotorB_t0;
Tess 0:6ca6892a17d0 73 int32_t positionmotorA_t_1;
Tess 0:6ca6892a17d0 74 int32_t positionmotorB_t_1;
Tess 0:6ca6892a17d0 75 int32_t positiondifference_motorA;
Tess 0:6ca6892a17d0 76 int32_t positiondifference_motorB;
Tess 0:6ca6892a17d0 77
Tess 0:6ca6892a17d0 78 /* inverse kinematica */
Tess 0:6ca6892a17d0 79 float dy; //dy waarde tussen -1 en 1 -1 -vmax; 1 vmax
Tess 0:6ca6892a17d0 80 float dx; //dx waarde tussen -1 en 1 -1 -vmax; 1 vmax
Tess 0:6ca6892a17d0 81 const float vmax = 0.08; // m/s
Tess 0:6ca6892a17d0 82 const float delta_t = 0.005; // 1/samplefrequentie, dus tijd tussen twee meetpunten
Tess 0:6ca6892a17d0 83 float X_positie;
Tess 0:6ca6892a17d0 84 float Y_positie;
Tess 0:6ca6892a17d0 85 float X_positie_begin;
Tess 0:6ca6892a17d0 86 float Y_positie_begin;
Tess 0:6ca6892a17d0 87 float puls_motorA;
Tess 0:6ca6892a17d0 88 float puls_motorB;
Tess 0:6ca6892a17d0 89 float kwadraat_X_positie;
Tess 0:6ca6892a17d0 90 float kwadraat_Y_positie;
Tess 0:6ca6892a17d0 91 float phi_A_pulsen_positie_begin;
Tess 0:6ca6892a17d0 92 float phi_B_pulsen_positie_begin;
Tess 0:6ca6892a17d0 93 float phi_A_positie_begin;
Tess 0:6ca6892a17d0 94 float phi_B_positie_begin;
Tess 0:6ca6892a17d0 95 float phi_1;
Tess 0:6ca6892a17d0 96 float lengte_arm = 276; // in mm anders rare imaginaire getallen
Tess 0:6ca6892a17d0 97 float phi_A;
Tess 0:6ca6892a17d0 98 float phi_B;
Tess 0:6ca6892a17d0 99 float Puls_motorA;
Tess 0:6ca6892a17d0 100 float Puls_motorB;
Tess 0:6ca6892a17d0 101 float phi_A_pulsen;
Tess 0:6ca6892a17d0 102 float phi_B_pulsen;
Tess 0:6ca6892a17d0 103 float pi = 3.14159265;
Tess 0:6ca6892a17d0 104
Tess 0:6ca6892a17d0 105 //START OF CODE
Tess 0:6ca6892a17d0 106
Tess 0:6ca6892a17d0 107 pc.baud(115200); // Set the baudrate (use this number in RealTerm too!)
Tess 0:6ca6892a17d0 108
Tess 0:6ca6892a17d0 109 // 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:6ca6892a17d0 110 // motor B zit onder en motor A zit boven en dus op zijn kop (en dus setpoint moet - zijn).
Tess 0:6ca6892a17d0 111
Tess 0:6ca6892a17d0 112 motordirB.write(0);
Tess 0:6ca6892a17d0 113 pwm_motorB.write(.1); //0.08
Tess 0:6ca6892a17d0 114 positionmotorB_t0 = motorB.getPosition();
Tess 0:6ca6892a17d0 115 do {
Tess 0:6ca6892a17d0 116 wait(0.2);
Tess 0:6ca6892a17d0 117 positionmotorB_t_1 = positionmotorB_t0 ;
Tess 0:6ca6892a17d0 118 positionmotorB_t0 = motorB.getPosition();
Tess 0:6ca6892a17d0 119 positiondifference_motorB = abs(positionmotorB_t0 - positionmotorB_t_1);
Tess 0:6ca6892a17d0 120 } while(positiondifference_motorB > 10);
Tess 0:6ca6892a17d0 121 motorB.setPosition(0);
Tess 0:6ca6892a17d0 122 pwm_motorB.write(0);
Tess 0:6ca6892a17d0 123
Tess 0:6ca6892a17d0 124 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 0:6ca6892a17d0 125
Tess 0:6ca6892a17d0 126 motordirA.write(1);
Tess 0:6ca6892a17d0 127 pwm_motorA.write(.1);
Tess 0:6ca6892a17d0 128 positionmotorA_t0 = motorA.getPosition();
Tess 0:6ca6892a17d0 129 do {
Tess 0:6ca6892a17d0 130 wait(0.2);
Tess 0:6ca6892a17d0 131 positionmotorA_t_1 = positionmotorA_t0 ;
Tess 0:6ca6892a17d0 132 positionmotorA_t0 = motorA.getPosition();
Tess 0:6ca6892a17d0 133 positiondifference_motorA = abs(positionmotorA_t0 - positionmotorA_t_1);
Tess 0:6ca6892a17d0 134 } while(positiondifference_motorA > 10);
Tess 0:6ca6892a17d0 135 motorA.setPosition(0);
Tess 0:6ca6892a17d0 136 pwm_motorA.write(0);
Tess 0:6ca6892a17d0 137
Tess 0:6ca6892a17d0 138 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 0:6ca6892a17d0 139
Tess 0:6ca6892a17d0 140 // 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:6ca6892a17d0 141
Tess 0:6ca6892a17d0 142 motordirA.write(0);
Tess 0:6ca6892a17d0 143 pwm_motorA.write(.08);
Tess 0:6ca6892a17d0 144 do {
Tess 0:6ca6892a17d0 145 wait(0.01);
Tess 0:6ca6892a17d0 146 setpoint_beginA = -63; // x-as
Tess 0:6ca6892a17d0 147 pwm_to_begin_motorA = (setpoint_beginA - motorA.getPosition()) *.001; // + omdat men met een negatieve hoekverdraaiing werkt.
Tess 0:6ca6892a17d0 148 keep_in_range(&pwm_to_begin_motorA, -0.3, 0.3 );
Tess 0:6ca6892a17d0 149 motordirA.write(0);
Tess 0:6ca6892a17d0 150 pwm_motorA.write(abs(pwm_to_begin_motorA));
Tess 0:6ca6892a17d0 151 pc.printf("s: %d, %f \n\r", motorA.getPosition(), pwm_to_begin_motorA);
Tess 0:6ca6892a17d0 152 } while(pwm_to_begin_motorA >= 0);
Tess 0:6ca6892a17d0 153 motorA.setPosition(0);
Tess 0:6ca6892a17d0 154 pwm_motorA.write(0);
Tess 0:6ca6892a17d0 155
Tess 0:6ca6892a17d0 156 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 0:6ca6892a17d0 157
Tess 0:6ca6892a17d0 158 // hierna moet motor A naar de rechtsonder A4. Motor A 532 (hoek 59.8 graden).
Tess 0:6ca6892a17d0 159
Tess 0:6ca6892a17d0 160 motordirA.write(0);
Tess 0:6ca6892a17d0 161 pwm_motorA.write(0.08);
Tess 0:6ca6892a17d0 162 do {
Tess 0:6ca6892a17d0 163 wait(0.01);
Tess 0:6ca6892a17d0 164 setpoint_rechtsonderA = -532; // rechtsonder positie A4
Tess 0:6ca6892a17d0 165 pwm_to_rechtsonder_motorA = (setpoint_rechtsonderA - motorA.getPosition()) *.001;
Tess 0:6ca6892a17d0 166 keep_in_range(&pwm_to_rechtsonder_motorA, -0.3, 0.3 );
Tess 0:6ca6892a17d0 167 motordirA.write(0);
Tess 0:6ca6892a17d0 168 pwm_motorA.write(abs(pwm_to_rechtsonder_motorA));
Tess 0:6ca6892a17d0 169 } while(pwm_to_rechtsonder_motorA >= 0);
Tess 0:6ca6892a17d0 170 pwm_motorA.write(0);
Tess 0:6ca6892a17d0 171
Tess 0:6ca6892a17d0 172 wait(1);
Tess 0:6ca6892a17d0 173
Tess 0:6ca6892a17d0 174 // Hierna moet motor B 21.6 (192) graden naar links om naar x-as te gaan.
Tess 0:6ca6892a17d0 175
Tess 0:6ca6892a17d0 176 motordirB.write(1);
Tess 0:6ca6892a17d0 177 pwm_motorB.write(.08);
Tess 0:6ca6892a17d0 178 do {
Tess 0:6ca6892a17d0 179 wait(0.01);
Tess 0:6ca6892a17d0 180 setpoint_beginB = 192; // x-as
Tess 0:6ca6892a17d0 181 pwm_to_begin_motorB = (setpoint_beginB - motorB.getPosition()) *.001;
Tess 0:6ca6892a17d0 182 keep_in_range(&pwm_to_begin_motorB, -0.3, 0.3 );
Tess 0:6ca6892a17d0 183 motordirB.write(1);
Tess 0:6ca6892a17d0 184 pwm_motorB.write(abs(pwm_to_begin_motorB));
Tess 0:6ca6892a17d0 185 } while(pwm_to_begin_motorB >= 0);
Tess 0:6ca6892a17d0 186 motorB.setPosition(0);
Tess 0:6ca6892a17d0 187 pwm_motorB.write(0);
Tess 0:6ca6892a17d0 188
Tess 0:6ca6892a17d0 189 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 0:6ca6892a17d0 190
Tess 0:6ca6892a17d0 191 // Hierna moet motor B van x-as naar de rechtsonder A4 positie. Motor B 268 (30.2 graden).
Tess 0:6ca6892a17d0 192
Tess 0:6ca6892a17d0 193 motordirB.write(1);
Tess 0:6ca6892a17d0 194 pwm_motorB.write(0.08);
Tess 0:6ca6892a17d0 195 do {
Tess 0:6ca6892a17d0 196 wait(0.01);
Tess 0:6ca6892a17d0 197 setpoint_rechtsonderB = 268; // rechtsonder positie A4
Tess 0:6ca6892a17d0 198 pwm_to_rechtsonder_motorB = (setpoint_rechtsonderB - motorB.getPosition()) *.001;
Tess 0:6ca6892a17d0 199 keep_in_range(&pwm_to_rechtsonder_motorB, -0.3, 0.3 );
Tess 0:6ca6892a17d0 200 motordirB.write(1);
Tess 0:6ca6892a17d0 201 pwm_motorB.write(abs(pwm_to_rechtsonder_motorB));
Tess 0:6ca6892a17d0 202 } while(pwm_to_rechtsonder_motorB >= 0);
Tess 0:6ca6892a17d0 203 pwm_motorB.write(0);
Tess 0:6ca6892a17d0 204
Tess 0:6ca6892a17d0 205 wait(1);
Tess 0:6ca6892a17d0 206
Tess 0:6ca6892a17d0 207 // Nu zijn de motoren gekalibreed en staan ze op de startpositie (rechtsonderhoek van A4).
Tess 0:6ca6892a17d0 208 // Hierna het script dat EMG wordt omgezet in een positie verandering
Tess 0:6ca6892a17d0 209
Tess 0:6ca6892a17d0 210 /*Create a ticker, and let it call the function 'setlooptimerflag' every 0.01s */
Tess 0:6ca6892a17d0 211 Ticker looptimer;
Tess 0:6ca6892a17d0 212 looptimer.attach(setlooptimerflag,0.01);
Tess 0:6ca6892a17d0 213
Tess 0:6ca6892a17d0 214 //INFINITE LOOP
Tess 0:6ca6892a17d0 215 while(1) {
Tess 0:6ca6892a17d0 216
Tess 0:6ca6892a17d0 217 while(looptimerflag != true);
Tess 0:6ca6892a17d0 218 looptimerflag = false;
Tess 0:6ca6892a17d0 219
Tess 0:6ca6892a17d0 220 dx = ((potmeterA.read()-0.5)*2000);
Tess 0:6ca6892a17d0 221 dy = ((potmeterB.read()-0.5)*2000); //waarde uit tussen -1 en 1
Tess 0:6ca6892a17d0 222
Tess 0:6ca6892a17d0 223 // inverse kinematica
Tess 0:6ca6892a17d0 224 phi_A_pulsen_positie_begin = motorA.getPosition();
Tess 0:6ca6892a17d0 225 phi_B_pulsen_positie_begin = motorB.getPosition();
Tess 0:6ca6892a17d0 226 //pc.printf("s: %f, %d \n\r", phi_A_pulsen_positie_begin, phi_B_pulsen_positie_begin);
Tess 0:6ca6892a17d0 227
Tess 0:6ca6892a17d0 228 phi_A_positie_begin = (360/3200.0) * phi_A_pulsen_positie_begin * (pi/180);
Tess 0:6ca6892a17d0 229 phi_B_positie_begin = (360/3200.0) * phi_B_pulsen_positie_begin * (pi/180);
Tess 0:6ca6892a17d0 230
Tess 0:6ca6892a17d0 231 phi_1 = phi_A_positie_begin - phi_B_positie_begin;
Tess 0:6ca6892a17d0 232
Tess 0:6ca6892a17d0 233 X_positie_begin = 2 * lengte_arm * sin(0.5 * phi_1) * cos(90 - 0.5 * phi_A_positie_begin - 0.5 * phi_B_positie_begin);
Tess 0:6ca6892a17d0 234 Y_positie_begin = 2 * lengte_arm * sin(0.5 * phi_1) * sin(90 - 0.5 * phi_A_positie_begin - 0.5 * phi_B_positie_begin);
Tess 0:6ca6892a17d0 235
Tess 0:6ca6892a17d0 236 X_positie = dx * vmax * delta_t + X_positie_begin; // delta_t eruit want dx tussen -1 en 1 en dimensieloos
Tess 0:6ca6892a17d0 237 Y_positie = dy * vmax * delta_t + Y_positie_begin;
Tess 0:6ca6892a17d0 238
Tess 0:6ca6892a17d0 239 kwadraat_X_positie = pow(X_positie,2);
Tess 0:6ca6892a17d0 240 kwadraat_Y_positie = pow(Y_positie,2);
Tess 0:6ca6892a17d0 241
Tess 0:6ca6892a17d0 242 phi_A = pi - acos(sqrt(kwadraat_X_positie+kwadraat_Y_positie)/(2*lengte_arm)) - atan(Y_positie/X_positie); //rad
Tess 0:6ca6892a17d0 243 phi_B = pi - phi_A - acos(-(kwadraat_X_positie + kwadraat_Y_positie) / (2 * pow(lengte_arm,2))+1);
Tess 0:6ca6892a17d0 244
Tess 0:6ca6892a17d0 245 phi_A_pulsen = (3200/(2*pi)) * phi_A;
Tess 0:6ca6892a17d0 246 phi_B_pulsen = (3200/(2*pi)) * phi_B;
Tess 0:6ca6892a17d0 247
Tess 0:6ca6892a17d0 248 // motor A moet de hoek altijd binnen 53.4 tot en met 124.3 graden liggen
Tess 0:6ca6892a17d0 249 // motor B moet de hoek altijd binnen 30.2 tot en met -16.5 graden liggen
Tess 0:6ca6892a17d0 250 keep_in_range(&phi_A_pulsen, -1104, -474); // voor motor moet bereik zijn -1104 tot -474
Tess 0:6ca6892a17d0 251 keep_in_range(&phi_B_pulsen, -146, 268); // voor motor moet bereik zijn -146 tot 268
Tess 0:6ca6892a17d0 252
Tess 0:6ca6892a17d0 253 Puls_motorA = phi_A_pulsen - phi_A_pulsen_positie_begin;
Tess 0:6ca6892a17d0 254 Puls_motorB = phi_B_pulsen - phi_B_pulsen_positie_begin;
Tess 0:6ca6892a17d0 255
Tess 0:6ca6892a17d0 256 // doe begin berekening
Tess 0:6ca6892a17d0 257 pwm_to_motorA = Puls_motorA*.001;
Tess 0:6ca6892a17d0 258 pwm_to_motorB = Puls_motorB*.001;
Tess 0:6ca6892a17d0 259
Tess 0:6ca6892a17d0 260 keep_in_range(&pwm_to_motorA, -1,1);
Tess 0:6ca6892a17d0 261 keep_in_range(&pwm_to_motorB, -1,1);
Tess 0:6ca6892a17d0 262
Tess 0:6ca6892a17d0 263 if(pwm_to_motorA > 0)
Tess 0:6ca6892a17d0 264 motordirA.write(1);
Tess 0:6ca6892a17d0 265 else
Tess 0:6ca6892a17d0 266 motordirA.write(0);
Tess 0:6ca6892a17d0 267 if(pwm_to_motorB > 0)
Tess 0:6ca6892a17d0 268 motordirB.write(1);
Tess 0:6ca6892a17d0 269 else
Tess 0:6ca6892a17d0 270 motordirB.write(0);
Tess 0:6ca6892a17d0 271
Tess 0:6ca6892a17d0 272 pwm_motorA.write(abs(pwm_to_motorA));
Tess 0:6ca6892a17d0 273 pwm_motorB.write(abs(pwm_to_motorB));
Tess 0:6ca6892a17d0 274
Tess 0:6ca6892a17d0 275
Tess 0:6ca6892a17d0 276 /*/PD regelaar voor motor A
Tess 0:6ca6892a17d0 277 wait(dt);
Tess 0:6ca6892a17d0 278 error_ti_A = phi_A_pulsen - motorA.getPosition(); // puls_motorA - motorA.getPosition();
Tess 0:6ca6892a17d0 279 P_regelaar_A = Kp * error_ti_A;
Tess 0:6ca6892a17d0 280 D_regelaar_A = Kd * ((error_ti_A - error_t0_A) / dt);
Tess 0:6ca6892a17d0 281 error_t0_A = error_ti_A;
Tess 0:6ca6892a17d0 282 output_regelaar_A = P_regelaar_A + D_regelaar_A;
Tess 0:6ca6892a17d0 283
Tess 0:6ca6892a17d0 284 //PD regelaar voor motor B
Tess 0:6ca6892a17d0 285 wait(dt);
Tess 0:6ca6892a17d0 286 error_ti_B = phi_B_pulsen - motorB.getPosition(); //puls_motorB - motorB.getPosition();
Tess 0:6ca6892a17d0 287 P_regelaar_B = Kp * error_ti_B;
Tess 0:6ca6892a17d0 288 D_regelaar_B = Kd * ((error_ti_B - error_t0_B) / dt);
Tess 0:6ca6892a17d0 289 error_t0_B = error_ti_B;
Tess 0:6ca6892a17d0 290 output_regelaar_B = P_regelaar_B + D_regelaar_B;
Tess 0:6ca6892a17d0 291 `*/
Tess 0:6ca6892a17d0 292 /* This is a PD-action! store in pwm_to_motor */
Tess 0:6ca6892a17d0 293 /*pwm_to_motorA = output_regelaar_A;
Tess 0:6ca6892a17d0 294 pwm_to_motorB = output_regelaar_B;
Tess 0:6ca6892a17d0 295
Tess 0:6ca6892a17d0 296 keep_in_range(&pwm_to_motorA, -1,1);
Tess 0:6ca6892a17d0 297 keep_in_range(&pwm_to_motorB, -1,1);
Tess 0:6ca6892a17d0 298
Tess 0:6ca6892a17d0 299 if(pwm_to_motorA > 0)
Tess 0:6ca6892a17d0 300 motordirA.write(1);
Tess 0:6ca6892a17d0 301 else
Tess 0:6ca6892a17d0 302 motordirA.write(0);
Tess 0:6ca6892a17d0 303 if(pwm_to_motorB > 0)
Tess 0:6ca6892a17d0 304 motordirB.write(1);
Tess 0:6ca6892a17d0 305 else
Tess 0:6ca6892a17d0 306 motordirB.write(0);
Tess 0:6ca6892a17d0 307
Tess 0:6ca6892a17d0 308 pwm_motorA.write(abs(pwm_to_motorA));
Tess 0:6ca6892a17d0 309 pwm_motorB.write(abs(pwm_to_motorB));*/
Tess 0:6ca6892a17d0 310 }
Tess 0:6ca6892a17d0 311 }
Tess 0:6ca6892a17d0 312
Tess 0:6ca6892a17d0 313 void keep_in_range(float * in, float min, float max)
Tess 0:6ca6892a17d0 314 {
Tess 0:6ca6892a17d0 315 *in > min ? *in < max? : *in = max: *in = min;
Tess 0:6ca6892a17d0 316 }
Tess 0:6ca6892a17d0 317