main.cpp

00001 #include "mbed.h"
00002 #include "encoder.h"
00003 #include "MODSERIAL.h"
00004 
00005 /*******************************************************************************
00006 *                                                                              *
00007 *   Code can be found at http://mbed.org/users/vsluiter/code/BMT-K9-Regelaar/  *
00008 *                                                                              *
00009 ********************************************************************************/
00010 
00011 /** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/
00012 void keep_in_range(float * in, float min, float max);
00013 
00014 /** variable to show when a new loop can be started*/
00015 /** volatile means that it can be changed in an    */
00016 /** interrupt routine, and that that change is vis-*/
00017 /** ible in the main loop. */
00018 
00019 volatile bool looptimerflag;
00020 
00021 /** function called by Ticker "looptimer"     */
00022 /** variable 'looptimerflag' is set to 'true' */
00023 /** each time the looptimer expires.          */
00024 void setlooptimerflag(void)
00025 {
00026     looptimerflag = true;
00027 }
00028 
00029 int main()
00030 {
00031     //LOCAL VARIABLES
00032     /*Potmeter input*/
00033     AnalogIn potmeterA(PTC2);
00034     AnalogIn potmeterB(PTB2);
00035     /* Encoder, using my encoder library */
00036     /* First pin should be PTDx or PTAx  */
00037     /* because those pins can be used as */
00038     /* InterruptIn                       */
00039     Encoder motorA(PTD4,PTC8);
00040     Encoder motorB(PTD0,PTD2);
00041     /* MODSERIAL to get non-blocking Serial*/
00042     MODSERIAL pc(USBTX,USBRX);
00043     /* PWM control to motor */
00044     PwmOut pwm_motorA(PTA12);
00045     PwmOut pwm_motorB(PTA5);
00046     /* Direction pin */
00047     DigitalOut motordirA(PTD3);
00048     DigitalOut motordirB(PTD1);
00049     /* variable to store setpoint in */
00050     float setpointA;
00051     float setpointB;
00052     float setpoint_beginA;
00053     float setpoint_beginB;
00054     float setpoint_rechtsonderA;
00055     float setpoint_rechtsonderB;
00056 
00057     /* variable to store pwm value in*/
00058     float pwm_to_motorA;
00059     float pwm_to_begin_motorA = 0;
00060     float pwm_to_begin_motorB = 0;
00061     float pwm_to_motorB;
00062     float pwm_to_rechtsonder_motorA;
00063     float pwm_to_rechtsonder_motorB;
00064 
00065     const float dt = 0.002;
00066     float Kp = 0.001;  //0.0208
00067     float Kd = 0.00004342;  //0.0006897
00068     float error_t0_A = 0;
00069     float error_t0_B = 0;
00070     float error_ti_A;
00071     float error_ti_B;
00072     float P_regelaar_A;
00073     float P_regelaar_B;
00074     float D_regelaar_A;
00075     float D_regelaar_B;
00076     float output_regelaar_A;
00077     float output_regelaar_B;
00078     
00079     int32_t positionmotorA_t0;
00080     int32_t positionmotorB_t0;
00081     int32_t positionmotorA_t_1;
00082     int32_t positionmotorB_t_1;
00083     int32_t positiondifference_motorA;
00084     int32_t positiondifference_motorB;
00085 
00086     //START OF CODE
00087 
00088     /*Set the baudrate (use this number in RealTerm too!) */
00089     pc.baud(921600);
00090 
00091     // 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.
00092     // motor B zit onder en motor A zit boven en dus op zijn kop (en dus setpoint moet - zijn).
00093 
00094     motordirB.write(0);
00095     pwm_motorB.write(.08);
00096     positionmotorB_t0 = motorB.getPosition();
00097     do {
00098         wait(0.2);
00099         positionmotorB_t_1 = positionmotorB_t0 ;
00100         positionmotorB_t0 = motorB.getPosition();
00101         positiondifference_motorB = abs(positionmotorB_t0 - positionmotorB_t_1);
00102     } while(positiondifference_motorB > 10);
00103     motorB.setPosition(0);
00104     pwm_motorB.write(0);
00105 
00106     wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
00107 
00108     motordirA.write(1);
00109     pwm_motorA.write(.08);
00110     positionmotorA_t0 = motorA.getPosition();
00111     do {
00112         wait(0.2);
00113         positionmotorA_t_1 = positionmotorA_t0 ;
00114         positionmotorA_t0 = motorA.getPosition();
00115         positiondifference_motorA = abs(positionmotorA_t0 - positionmotorA_t_1);
00116     } while(positiondifference_motorA > 10);
00117     motorA.setPosition(0);
00118     pwm_motorA.write(0);
00119 
00120     wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
00121 
00122     // 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.
00123 
00124     motordirA.write(0);
00125     pwm_motorA.write(.08);
00126     do {
00127         setpoint_beginA = -63;      // x-as
00128         pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001);   // + omdat men met een negatieve hoekverdraaiing werkt.
00129         wait(0.2);
00130         keep_in_range(&pwm_to_begin_motorA, -1, 1 );
00131         motordirA.write(0);
00132         pwm_motorA.write(pwm_to_begin_motorA);
00133     } while(pwm_to_begin_motorA <= 0);
00134     motorA.setPosition(0);
00135     pwm_motorA.write(0);
00136 
00137     wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
00138 
00139     // hierna moet motor A naar de rechtsonder A4. Motor A 532.
00140 
00141     motordirA.write(0);
00142     pwm_motorA.write(0.08);
00143     do {
00144         setpoint_rechtsonderA = -532;     // rechtsonder positie A4
00145         pwm_to_rechtsonder_motorA = abs((setpoint_rechtsonderA + motorA.getPosition()) *.001);
00146         wait(0.2);
00147         keep_in_range(&pwm_to_rechtsonder_motorA, -1, 1 );
00148         motordirA.write(0);
00149         pwm_motorA.write(pwm_to_rechtsonder_motorA);
00150     } while(pwm_to_rechtsonder_motorA <= 0);
00151     pwm_motorA.write(0);
00152 
00153     wait(1);
00154 
00155     // Hierna moet motor B 21.6 (192) graden naar links om naar x-as te gaan.
00156 
00157     motordirB.write(1);
00158     pwm_motorB.write(.08);
00159     do {
00160         setpoint_beginB = 192;      // x-as
00161         pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
00162         wait(0.2);
00163         keep_in_range(&pwm_to_begin_motorB, -1, 1 );
00164         motordirB.write(1);
00165         pwm_motorB.write(pwm_to_begin_motorB);
00166     } while(pwm_to_begin_motorB <= 0);
00167     motorB.setPosition(0);
00168     pwm_motorB.write(0);
00169 
00170     wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
00171 
00172     // Hierna moet motor B van x-as naar de rechtsonder A4 positie. Motor B 460.
00173 
00174     motordirB.write(1);
00175     pwm_motorB.write(0.08);
00176     do {
00177         setpoint_rechtsonderB = 460;      // rechtsonder positie A4
00178         pwm_to_rechtsonder_motorB = abs((setpoint_rechtsonderB - motorB.getPosition()) *.001);
00179         wait(0.2);
00180         keep_in_range(&pwm_to_rechtsonder_motorB, -1, 1 );
00181         motordirB.write(1);
00182         pwm_motorB.write(pwm_to_rechtsonder_motorB);
00183     } while(pwm_to_rechtsonder_motorB <= 0);
00184     pwm_motorB.write(0);
00185 
00186     wait(1);
00187 
00188     // Nu zijn de motoren gekalibreed en staan ze op de startpositie.
00189     // Hierna het script dat EMG wordt omgezet in een positie verandering
00190 
00191     /*Create a ticker, and let it call the     */
00192     /*function 'setlooptimerflag' every 0.01s  */
00193     Ticker looptimer;
00194     looptimer.attach(setlooptimerflag,0.01);
00195 
00196     //INFINITE LOOP
00197     while(1) {
00198 
00199         while(looptimerflag != true);
00200         looptimerflag = false;
00201 
00202         // hier EMG
00203         //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
00204         //setpointB = (potmeterB.read())*(415);           // bereik van 46.7 graden
00205         //pc.printf("s: %f, %d ", setpointA, motorA.getPosition());
00206         //pc.printf("s: %f, %d ", setpointB, motorB.getPosition());
00207         setpointA = (potmeterA.read() - 0.5)*(631/2);
00208         setpointB = (potmeterB.read() - 0.5) * (871/2);
00209 
00210         // motor A moet de hoek altijd binnen 53.4 tot en met 124.3 graden liggen
00211         // motor B moet de hoek altijd binnen 30.2 tot en met -16.5 graden liggen
00212         /*keep_in_range(&setpointA, -1105, -474);     // voor motor moet bereik zijn -1105 tot -474
00213         keep_in_range(&setpointB, -147, 269);       // voor motor moet bereik zijn -147 tot 269
00214         */
00215 
00216         //PD regelaar voor motor A
00217         wait(dt);
00218         error_ti_A = setpointA - motorA.getPosition();
00219         P_regelaar_A = Kp * error_ti_A;
00220         D_regelaar_A = Kd * ((error_ti_A - error_t0_A) / dt);
00221         error_t0_A = error_ti_A;
00222         output_regelaar_A = P_regelaar_A + D_regelaar_A;
00223 
00224         //PD regelaar voor motor B
00225         wait(dt);
00226         error_ti_B = setpointB - motorB.getPosition();
00227         P_regelaar_B = Kp * error_ti_B;
00228         D_regelaar_B = Kd * ((error_ti_B - error_t0_B) / dt);
00229         error_t0_B = error_ti_B;
00230         output_regelaar_B = P_regelaar_B + D_regelaar_B;
00231 
00232         /* This is a PID-action! store in pwm_to_motor */
00233         pwm_to_motorA = output_regelaar_A;
00234         pwm_to_motorB = output_regelaar_B;
00235 
00236         keep_in_range(&pwm_to_motorA, -1,1);
00237         keep_in_range(&pwm_to_motorB, -1,1);
00238 
00239         if(pwm_to_motorA > 0)
00240             motordirA.write(1);
00241         else
00242             motordirA.write(0);
00243         if(pwm_to_motorB > 0)
00244             motordirB.write(1);
00245         else
00246             motordirB.write(0);
00247 
00248         pwm_motorA.write(abs(pwm_to_motorA));
00249         pwm_motorB.write(abs(pwm_to_motorB));
00250     }
00251 }
00252 
00253 
00254 void keep_in_range(float * in, float min, float max)
00255 {
00256 *in > min ? *in < max? : *in = max: *in = min;
00257 }
00258 
00259 
00260 
00261 
00262 
00263 
00264