main.cpp

00001 /***************************************/
00002 /*                                     */
00003 /*   BRONCODE GROEP 5, MODULE 9, 2014  */
00004 /*       *****-THE SLAP-******         */
00005 /*                                     */
00006 /* -Dominique Clevers                  */
00007 /* -Rianne van Dommelen                */
00008 /* -Daan de Muinck Keizer              */
00009 /* -David den Houting                  */
00010 /* -Marjolein Thijssen                 */
00011 /***************************************/
00012 #include "mbed.h"
00013 #include "encoder.h"
00014 #include "MODSERIAL.h"
00015 #include "HIDScope.h"
00016 
00017 
00018 //MAXIMALE UITWIJKING BEPALEN
00019 /** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/
00020 void keep_in_range(float * in, float min, float max);
00021 
00022 /** variable to show when a new loop can be started*/
00023 /** volatile means that it can be changed in an    */
00024 /** interrupt routine, and that that change is vis-*/
00025 /** ible in the main loop. */
00026 
00027 volatile bool looptimerflag;
00028 
00029 /** function called by Ticker "looptimer"     */
00030 /** variable 'looptimerflag' is set to 'true' */
00031 /** each time the looptimer expires.          */
00032 void setlooptimerflag(void)
00033 {
00034     looptimerflag = true;
00035 }
00036 
00037 int main() {
00038     //LOCAL VARIABLES 
00039     /*Potmeter input*/
00040     AnalogIn potmeter(PTC2);
00041     /* Encoder, using my encoder library */
00042     /* First pin should be PTDx or PTAx  */
00043     /* because those pins can be used as */
00044     /* InterruptIn                       */
00045     Encoder motor1(PTD0,PTC9);
00046     /* MODSERIAL to get non-blocking Serial*/
00047     MODSERIAL pc(USBTX,USBRX);
00048     /* PWM control to motor */
00049     PwmOut pwm_motor(PTA12);
00050     /* Direction pin */
00051     DigitalOut motordir(PTD3);
00052     /* variable to store setpoint in */
00053     float setpoint;
00054     /* variable to store pwm value in*/
00055     float pwm_to_motor;
00056     //START OF CODE
00057     
00058     /*Set the baudrate (use this number in RealTerm too! */
00059     pc.baud(230400);
00060     
00061    /*Create a ticker, and let it call the     */
00062    /*function 'setlooptimerflag' every 0.01s  */
00063     Ticker looptimer;
00064     looptimer.attach(setlooptimerflag,0.01);  
00065     
00066     //INFINITE LOOP 
00067     while(1) {
00068         /* Wait until looptimer flag is true. */
00069         /* '!=' means not-is-equal            */
00070         while(looptimerflag != true);
00071         /* Clear the looptimerflag, otherwise */
00072         /* the program would simply continue  */
00073         /* without waitingin the next iteration*/
00074         looptimerflag = false;
00075         
00076         /* Read potmeter value, apply some math */
00077         /* to get useful setpoint value         */
00078         setpoint = (potmeter.read()-0.5)*2000;
00079         
00080         /* Print setpoint and current position to serial terminal*/
00081         pc.printf("s: %f, %d \n\r", setpoint, motor1.getPosition());
00082         
00083         /* This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor */
00084         pwm_to_motor = (setpoint - motor1.getPosition())*.001;
00085         /* Coerce pwm value if outside range          */
00086         /* Not strictly needed here, but useful       */
00087         /* if doing other calculations with pwm value */
00088         keep_in_range(&pwm_to_motor, -1,1);
00089         
00090         /* Control the motor direction pin. based on   */
00091         /* the sign of your pwm value.      If your    */
00092         /* motor keeps spinning when running this code */
00093         /* you probably need to swap the motor wires,  */
00094         /* or swap the 'write(1)' and 'write(0)' below */
00095         if(pwm_to_motor > 0)
00096             motordir.write(1);
00097         else
00098             motordir.write(0);
00099         //WRITE VALUE TO MOTOR
00100         /* Take the absolute value of the PWM to send */
00101         /* to the motor. */  
00102         pwm_motor.write(abs(pwm_to_motor));
00103     }
00104 }
00105 
00106 
00107 //coerces value 'in' to min or max when exceeding those values
00108 //if you'd like to understand the statement below take a google for
00109 //'ternary operators'.
00110 void keep_in_range(float * in, float min, float max)
00111 {
00112     *in > min ? *in < max? : *in = max: *in = min;
00113 }
00114 
00115 
00116 //AANSTUREN MOTOR (POSITIE EN SNELHEID)
00117 #define TSAMP 0.01
00118 #define K_P (0.1)
00119 #define K_I (0.03  *TSAMP)
00120 #define K_D (0.001 /TSAMP)
00121 #define I_LIMIT 1.
00122 
00123 #define M1_PWM PTC8
00124 #define M1_DIR PTC9
00125 #define M2_PWM PTA5
00126 #define M2_DIR PTA4
00127 
00128 //#define POT_AVG 50
00129 
00130 void clamp(float * in, float min, float max);
00131 float pid(float setpoint, float measurement);
00132 volatile bool looptimerflag;
00133 //float potsamples[POT_AVG];
00134 HIDScope scope(6);
00135 
00136 
00137 void setlooptimerflag(void)
00138 {
00139     looptimerflag = true;
00140 }
00141 
00142 int main()
00143 {
00144     AnalogIn potmeter(PTC2);
00145     //start Encoder-> first pin should be PTDx or PTAx, second pin doesn't matter
00146     Encoder motor1(PTD0,PTA13);
00147     /*PwmOut to motor driver*/
00148     PwmOut pwm_motor(M2_PWM);
00149     //10kHz PWM frequency
00150     pwm_motor.period_us(75);
00151     DigitalOut motordir(M2_DIR);
00152     Ticker looptimer;
00153     looptimer.attach(setlooptimerflag,TSAMP);
00154     while(1) {
00155         int16_t setpoint;
00156         float new_pwm;
00157         /*wait until timer has elapsed*/
00158         while(!looptimerflag);
00159         looptimerflag = false; //clear flag
00160         /*potmeter value: 0-1*/
00161         setpoint = (potmeter.read()-.5)*500;        
00162         /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/
00163         new_pwm = pid(setpoint, motor1.getPosition());
00164         clamp(&new_pwm, -1,1);
00165         scope.set(0, setpoint);
00166         scope.set(4, new_pwm);
00167         scope.set(5, motor1.getPosition());
00168         // ch 1, 2 and 3 set in pid controller */
00169         scope.send();
00170         if(new_pwm < 0)
00171             motordir = 0;
00172         else
00173             motordir = 1;
00174         pwm_motor.write(abs(new_pwm));
00175     }
00176 }
00177 
00178 
00179 //clamps value 'in' to min or max when exceeding those values
00180 //if you'd like to understand the statement below take a google for
00181 //'ternary operators'.
00182 void clamp(float * in, float min, float max)
00183 {
00184     *in > min ? *in < max? : *in = max: *in = min;
00185 }
00186 
00187 
00188 float pid(float setpoint, float measurement)
00189 {
00190     float error;
00191     static float prev_error = 0;
00192     float           out_p = 0;
00193     static float    out_i = 0;
00194     float           out_d = 0;
00195     error  = setpoint-measurement;
00196     out_p  = error*K_P;
00197     out_i += error*K_I;
00198     out_d  = (error-prev_error)*K_D;
00199     clamp(&out_i,-I_LIMIT,I_LIMIT);
00200     prev_error = error;
00201     scope.set(1,out_p);
00202     scope.set(2,out_i);
00203     scope.set(3,out_d);
00204     return out_p + out_i + out_d;
00205 }
00206 
00207 
00208 //POSITIE EN SNELHEID UITLEZEN OP PC
00209 int main()
00210 {
00211 while(1) //Loop
00212     {
00213     /** Make encoder object.
00214     * First pin should be on PTAx or PTDx because those pins can be used as InterruptIn
00215     * Second pin can be any digital input
00216     */
00217     Encoder motor1(PTD0,PTC9);
00218     /*Use USB serial to send values*/
00219     MODSERIAL pc(USBTX,USBRX);
00220     /*Set baud rate to 115200*/
00221     pc.baud(115200);
00222     while(1) { //Loop
00223         /**Wait to prevent buffer overflow, and to keep terminal readable (not scrolling too fast)*/
00224         wait(0.2);
00225         /** print position (integer) and speed (float) to the PC*/
00226         pc.printf("pos: %d, speed %f \r\n",motor1.getPosition(), motor1.getSpeed());
00227     }
00228 }