main.cpp

00001 #include "mbed.h"
00002 #include "FastPWM.h"
00003 #include "HIDScope.h"
00004 #include "QEI.h"
00005 #include "BiQuad.h"
00006 #define SERIAL_BAUD 115200  // baud rate for serial communication
00007 
00008 Serial pc(USBTX,USBRX);
00009 
00010 // Setup Pins
00011 // Note: Pin D10 and D11 for encoder, D4-D7 for motor controller
00012 // Potmeter 1 gives the reference position x
00013 AnalogIn pot1(A3);
00014 // Potmeter 2 gives the reference position y
00015 AnalogIn pot2(A4);
00016 
00017 // Setup Buttons
00018 InterruptIn button1(PTB9);                  // button 1
00019 InterruptIn button2(PTA1);                  // button 2
00020 InterruptIn button3(PTC6);                 // SW2
00021 InterruptIn button4(PTA4);                 // SW3
00022 
00023 //
00024 volatile bool button1_value = false;
00025 
00026 // Set motor Pinouts
00027 DigitalOut motor1_dir(D4);
00028 FastPWM motor1_pwm(D5);
00029 DigitalOut motor2_dir(D7);
00030 FastPWM motor2_pwm(D6);
00031 
00032 // Set LED pins
00033 DigitalOut led(LED_RED);
00034 
00035 // Set HID scope
00036 HIDScope    scope(6);
00037 
00038 // Set encoder
00039 QEI m1_EncoderCW(D10,D11,NC,32);
00040 QEI m1_EncoderCCW(D11,D10,NC,32);
00041 QEI m2_EncoderCW(D13,D12,NC,32);
00042 QEI m2_EncoderCCW(D12,D13,NC,32);
00043 
00044 volatile const int   COUNTS_PER_REV = 8400;
00045 volatile const float DEGREES_PER_PULSE = 8400 / 360;
00046 volatile const float CONTROLLER_TS = 0.01; // 1000 Hz
00047 
00048 // Set initial Kp and Ki
00049 volatile double Kp = 0.01; // last known good Kp: 0.01
00050 volatile double Ki = 0.0;   // last known good Ki: 0.0025
00051 volatile double Kd = 0.0; // last known good Kd: 0.0
00052 
00053 volatile double Ts = 0.01;
00054 volatile double N = 100;
00055 
00056 // Memory values for controllers
00057 double m1_v1 = 0, m1_v2 = 0;
00058 double m2_v1 = 0, m2_v2 = 0;
00059 
00060 // Set default mode
00061 volatile int Mode = 0;
00062 
00063 // Variabelen voor MotorHoekBerekenen
00064 volatile double x = 0.0;                             // beginpositie x en y
00065 volatile double y = 305.5;
00066 volatile const double pi = 3.14159265359; 
00067 volatile double Theta1Gear = 60.0;              // Beginpositie op 60 graden
00068 volatile double Theta2Gear = 60.0;
00069 volatile double Theta1 = Theta1Gear*42/12;           // Beginpositie van MotorHoek
00070 volatile double Theta2 = Theta2Gear*42/12;            // frequentie in Hz waarmee theta wordt uigereken
00071 
00072 // Functions for angle calculation
00073 double Calc_Prev_x () {
00074     double Prev_x = x;
00075         //pc.printf("prev x = %f\r\n", Prev_x);
00076     return Prev_x;
00077 }
00078 
00079 // vorige y opslaan
00080 double Calc_Prev_y () {
00081     double Prev_y = y;
00082         //pc.printf("prev y = %f\r\n", Prev_y);
00083     return Prev_y;
00084 }
00085 
00086 // vorige Theta1Gear opslaan
00087 double Calc_Prev_Theta1_Gear () {
00088     double Prev_Theta1_Gear = Theta1Gear;
00089     return Prev_Theta1_Gear;
00090 }
00091 
00092 //vorige Theta2Gear opslaan
00093 double Calc_Prev_Theta2_Gear () {
00094     double Prev_Theta2_Gear = Theta2Gear;
00095     return Prev_Theta2_Gear;
00096 }
00097 
00098 void CalcXY()
00099 {
00100     /*
00101     double Step = 5/Fr_Speed_Theta ;  //10 mm per seconde afleggen
00102     
00103     if (BicepsLeft==true && BicepsRight==true && Leg==true && Stepper_State==false) {   
00104         Stepper_State = true;     // we zijn aan het flipper dus geen andere dingen doen
00105         FunctieFlipper() ;
00106     }
00107     else if (BicepsLeft==true && BicepsRight==false && Leg==false && Stepper_State==false) {
00108         x = x - Step;
00109     }
00110     else if (BicepsLeft==false && BicepsRight==true && Leg==false && Stepper_State==false) {
00111         x = x + Step;       // naar Right bewegen
00112     }
00113     else if (BicepsLeft==true && BicepsRight==true && Leg==false && Stepper_State==false) {
00114         y = y + Step;        // naar voren bewegen
00115     }
00116     else if (BicepsLeft==false && BicepsRight==true && Leg==true && Stepper_State==false) {
00117         y = y - Step;       // naar achter bewegen
00118     }
00119     else if (BicepsLeft==false && BicepsRight==false && Leg==false || Stepper_State==true) {
00120     }
00121     */
00122     
00123     x = (pot1.read()-0.5f)*400.0f; // x is een waarde tussen de -200 en 200
00124     y = 50.0f+(pot2.read()*256.0f);
00125     
00126     // Grenswaardes     LET OP: ARMEN MISSCHIEN GEBLOKKEERD DOOR BALK AAN DE BINNENKANT
00127     if (x > 200) {
00128         x = 200;
00129     }
00130     else if (x < -200) {
00131         x = -200;
00132     }
00133     if (y > 306) {
00134         y = 306;
00135     }
00136     else if (y < 50) {
00137         y = 50;         // GOKJE, UITPROBEREN
00138     }
00139     //pc.printf("x = %f, y = %f\r\n", x, y);
00140       
00141     //scope.set(2,x);
00142     //scope.set(3,y);
00143 }    
00144 
00145 // diagonaal berekenen voor linker arm
00146 double CalcDia1 (double x, double y) {
00147     double a = 50.0;                                // de afstand van gekozen assenstelsel tot de armas (assenstelsel precies in het midden)   KEERTJE NAMETEN
00148     double BV1 = sqrt(pow((a+x),2) + pow(y,2));     // diagonaal (afstand van armas tot locatie) berekenen
00149     double Dia1 = pow(BV1,2)/(2*BV1);               // berekenen van de afstand oorsprong tot diagonaal
00150     
00151     //pc.printf("dia = %f, x = %f, y= %f\r\n", Dia1, x, y);
00152     return Dia1;
00153 }
00154 
00155 // diagonaal berekenen voor rechter arm
00156 double CalcDia2 (double x, double y) {
00157     double a = 50.0;                                
00158     double BV2 = sqrt(pow((x-a),2) + pow(y,2));     // zelfde nog een keer doen maar nu voor de rechter arm 
00159     double Dia2 = pow(BV2,2)/(2*BV2); 
00160     
00161     //pc.printf("dia = %f, x = %f, y= %f\r\n", Dia2, x, y);
00162     return Dia2;
00163 }
00164 
00165 // calculate Theta1
00166 void CalcTheta1 (double Dia1) {
00167     double a = 50.0; 
00168     double Bar = 200.0;    // lengte van de armen 
00169     
00170     // Hoek berekenen van het grote tandwiel (gear)
00171     if (x > -a) {
00172         Theta1Gear = pi - atan(y/(x+a)) - acos(Dia1/Bar);
00173     }
00174     else if (x > -a) {
00175         Theta1Gear = pi - (pi + atan(y/(x+a))) - acos(Dia1/Bar);
00176     }
00177     else {  // als x=-a
00178         Theta1Gear = 0.5*pi - acos(Dia1/Bar);
00179     }
00180     Theta1Gear = Theta1Gear*180.0/pi;       // veranderen van radialen naar graden
00181              
00182     // omrekenen van grote tandwiel naar motortandwiel
00183     Theta1 = Theta1Gear*42.0/12.0;  // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12. 
00184         
00185     // pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta1, Theta1Gear);
00186 }
00187 
00188 void CalcTheta2 (double Dia2) {
00189     double a = 50.0; 
00190     double Bar = 200.0;    // lengte van de armen 
00191     double Prev_Theta2_Gear = Theta2Gear; 
00192         
00193     // Hoek berekenen van het grote tandwiel (gear)
00194     if (x < a) {
00195         Theta2Gear = pi + atan(y/(x-a)) - acos(Dia2/Bar);
00196     }
00197     else if (x > a) {
00198         Theta2Gear = pi - (pi - atan(y/(x-a))) - acos(Dia2/Bar);
00199     }
00200     else {  // als x=a
00201         Theta2Gear = 0.5*pi - acos(Dia2/Bar);
00202     }
00203     Theta2Gear = Theta2Gear*180/pi;         // veranderen van radialen naar graden
00204     
00205     // omrekenen van grote tandwiel naar motortandwiel
00206     Theta2 = Theta2Gear*42.0/12.0;  // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12.
00207 
00208     // pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta2, Theta2Gear);
00209 }
00210 
00211 // als een van de hoeken te groot wordt, zet dan alles terug naar de vorige positie
00212 void AngleLimits (double Prev_Theta1_Gear, double Prev_Theta2_Gear, double Prev_x, double Prev_y) {
00213     double MaxThetaGear = 100.0;   // de hoek voordat arm het opstakel raakt (max hoek is 107.62 tussen arm en opstakel, maken er 100 van voor veiligheid)
00214     
00215     if (Theta1Gear >= MaxThetaGear || Theta2Gear >= MaxThetaGear) {
00216         Theta1Gear = Prev_Theta1_Gear;
00217         Theta2Gear = Prev_Theta2_Gear;
00218         x = Prev_x;
00219         y = Prev_y;
00220         
00221         Theta1 = Theta1Gear*42.0/12.0;
00222         Theta2 = Theta2Gear*42.0/12.0; 
00223     }
00224     // pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta2, Theta2Gear);
00225 }
00226 
00227 void CalculationsForTheta () {
00228     double Prev_x = Calc_Prev_x ();
00229     double Prev_y = Calc_Prev_y ();
00230     double Calc_Prev_Theta1_Gear ();
00231     double Calc_Prev_Theta2_Gear ();
00232     CalcXY();
00233     double Dia1 = CalcDia1 (x, y);
00234     double Dia2 = CalcDia2 (x, y);
00235     CalcTheta1 (Dia1);
00236     CalcTheta2 (Dia2);
00237     AngleLimits (Theta1Gear, Theta2Gear, Prev_x, Prev_y);       // laatste check
00238     // hier mag je de motor gaan aansturen
00239     //scope.send();
00240 }
00241 
00242 void Set_Mode()
00243 {
00244     Mode++;
00245     if (Mode == 3)
00246     {
00247         Mode = 0;
00248     }
00249     pc.printf("\r 0: Kp \r\n");
00250     pc.printf("\r 1: Ki \r\n");
00251     pc.printf("\r 2: Kd \r\n");
00252     pc.printf("\r MODE = %i \r\n",Mode);
00253 }
00254 
00255 void increase()
00256 {
00257     switch(Mode)
00258     {
00259         case 0:
00260             Kp = Kp + 0.01f;
00261             break;
00262         case 1:
00263             Ki = Ki + 0.00001f;
00264             break;
00265         case 2:
00266             Kd = Kd + 0.01f;
00267             break;
00268         default:
00269             pc.printf("\r swtich error \r\n");
00270     }
00271     pc.printf("\r Kp: %f, Ki: %f, Kd: %f \r\n",Kp,Ki,Kd);
00272 }
00273 
00274 void decrease()
00275 {
00276     switch(Mode)
00277     {
00278         case 0:
00279             if (Kp <= 0.0f)
00280             {
00281                 Kp = 0.0f;
00282                 break;
00283             }
00284             Kp = Kp - 0.01f;
00285             break;
00286         case 1:
00287             if (Ki == 0.0f)
00288             {
00289                 Ki = 0.0f;
00290                 break;
00291             }
00292             Ki = Ki - 0.00001f;
00293             break;
00294         case 2:
00295             if (Kd == 0.0f)
00296             {
00297                 Kd = 0.0f;
00298                 break;
00299             }
00300             Kd = Kd - 0.01f;
00301             break;
00302         default:
00303             pc.printf("\r swtich error \r\n");
00304     }
00305     pc.printf("\r Kp: %f, Ki: %f, Kd: %f \r\n",Kp,Ki,Kd);
00306 }
00307 
00308 double m1_GetPosition()
00309 {
00310     int countsCW = m1_EncoderCW.getPulses();
00311     int countsCCW= m1_EncoderCCW.getPulses();
00312     int net_counts=countsCW-countsCCW;
00313     double Position=(net_counts*360.0)/COUNTS_PER_REV;
00314     return Position;
00315 }
00316 
00317 double m2_GetPosition()
00318 {
00319     int countsCW = m2_EncoderCW.getPulses();
00320     int countsCCW= m2_EncoderCCW.getPulses();
00321     int net_counts=countsCW-countsCCW;
00322     double Position=(net_counts*360.0)/COUNTS_PER_REV;
00323     return Position;
00324 }
00325 
00326 double m1_GetPosition_cal()
00327 {
00328     int countsCW = m1_EncoderCW.getPulses();
00329     int countsCCW= m1_EncoderCCW.getPulses();
00330     int net_counts=countsCW-countsCCW;
00331     double Position=(net_counts*360.0)/COUNTS_PER_REV+210.0f; // calibrated position is 210 degrees
00332     return Position;
00333 }
00334 
00335 double m2_GetPosition_cal()
00336 {
00337     int countsCW = m2_EncoderCW.getPulses();
00338     int countsCCW= m2_EncoderCCW.getPulses();
00339     int net_counts=countsCW-countsCCW;
00340     double Position=(net_counts*360.0)/COUNTS_PER_REV+210.0f; // calibrated position is 210 degrees
00341     return Position;
00342 }
00343 double m1_PID(double error, const double Kp, const double Ki, const double Kd, const double Ts, const double N, double &m1_v1, double &m1_v2)
00344 {
00345     double a1 = -4/(N*Ts+2),
00346     a2 = -1*(N*Ts - 2)/(N*Ts+2), 
00347     b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4),
00348     b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2),
00349     b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
00350     
00351     double v = error - a1*m1_v1 - a2*m1_v2;
00352     double u = b0*v + b1*m1_v1 + b2*m1_v2;
00353     m1_v2 = m1_v1; m1_v1 = v;
00354     return u;
00355 }
00356 
00357 double m2_PID(double error, const double Kp, const double Ki, const double Kd, const double Ts, const double N, double &m2_v1, double &m2_v2)
00358 {
00359     double a1 = -4/(N*Ts+2),
00360     a2 = -1*(N*Ts - 2)/(N*Ts+2), 
00361     b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4),
00362     b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2),
00363     b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
00364     
00365     double v = error - a1*m1_v1 - a2*m1_v2;
00366     double u = b0*v + b1*m1_v1 + b2*m1_v2;
00367     m1_v2 = m1_v1; m1_v1 = v;
00368     return u;
00369 }
00370 
00371 
00372 
00373 void SetMotor(int motor_number, double MotorValue)
00374 {
00375     // Given -1<=motorValue<=1, this sets the PWM and direction
00376     // bits for motor 1. Positive value makes motor rotating
00377     // clockwise. motorValues outside range are truncated to
00378     // within range
00379     if (motor_number == 1)
00380     {
00381         if (MotorValue >=0)
00382         {
00383             motor1_dir=0;
00384         }
00385         else
00386         {
00387             motor1_dir=1;
00388         }
00389         if (fabs(MotorValue)>1){
00390             motor1_pwm.write(1);
00391         }
00392         else
00393         {
00394             motor1_pwm.write(abs(MotorValue));
00395         }
00396     }
00397     else
00398     {  
00399         if (MotorValue >=0)
00400         {
00401             motor2_dir=0;
00402         }
00403         else
00404         {
00405             motor2_dir=1;
00406         }
00407         if (fabs(MotorValue)>1){
00408             motor2_pwm.write(1);
00409         }
00410         else
00411         {
00412             motor2_pwm.write(abs(MotorValue));
00413         }
00414     }   
00415 }
00416 
00417 void BlinkLed(){
00418     led = not led;
00419 }
00420 
00421 void Controller_motor()
00422 {
00423     // calculate the reference position
00424     CalculationsForTheta();
00425     // get the actual position
00426     double m1_Position = m1_GetPosition_cal();
00427     double m2_Position = m2_GetPosition_cal();
00428     // Set position scopes
00429     scope.set(0,x);
00430     scope.set(1,y);
00431     scope.set(2,Theta1);
00432     scope.set(3,Theta2);
00433     /*
00434     scope.set(0,Theta1);
00435     scope.set(1,m1_Position);
00436     scope.set(3,Theta2);
00437     scope.set(4,m2_Position);
00438     */
00439     //scope.set(2,m1_Position);
00440     //scope.set(4,m2_Position);
00441     // calc the error
00442     double m1_error = Theta1 - m1_Position;
00443     double m2_error = Theta2 - m2_Position;
00444     //scope.set(2,m1_error);
00445     //scope.set(6,m2_error);
00446     // calc motorvalues for controller;
00447     double m1_MotorValue = m1_PID(m1_error, Kp, Ki, Kd, Ts, N, m1_v1, m1_v2);
00448     double m2_MotorValue = m2_PID(m2_error, Kp, Ki, Kd, Ts, N, m2_v1, m2_v2);
00449     scope.set(4,m1_MotorValue);
00450     scope.set(5,m2_MotorValue);
00451     // Set the motorvalues
00452     SetMotor(1, m1_MotorValue);
00453     SetMotor(2, m2_MotorValue);
00454     // Set motorvalues for scope
00455     // Send data to HIDScope
00456     scope.send();
00457 }
00458 
00459 void button1_switch()
00460 {
00461     button1_value = not button1_value;
00462 }
00463 
00464 void PotControl()
00465 {
00466     double Motor1_Value = (pot1.read() - 0.5f)/2.0f;
00467     double Motor2_Value = (pot2.read() - 0.5f)/2.0f;
00468     //pc.printf("\r\n Motor value 1: %f \r\n",Motor1_Value);
00469     //pc.printf("\r\n Motor value 2: %f \r\n",Motor2_Value);
00470     double m1_Position = m1_GetPosition();
00471     double m2_Position = m2_GetPosition();
00472     
00473     
00474     scope.set(0, Motor1_Value);
00475     scope.set(1, m1_Position);
00476     scope.set(2, Motor2_Value);
00477     scope.set(3, m2_Position);
00478     scope.send();
00479     // Write the motors
00480     SetMotor(1, Motor1_Value);
00481     SetMotor(2, Motor2_Value);
00482 }
00483  
00484 /*
00485 void CalculateSpeed() {
00486     countsCW = EncoderCW.getPulses();
00487     countsCCW= EncoderCCW.getPulses();
00488     net_counts=countsCW-countsCCW;
00489     degrees=(net_counts*360.0)/counts_per_rev;
00490     
00491     curr_degrees = degrees;
00492     speed = (curr_degrees-prev_degrees)/T_CalculateSpeed; 
00493     prev_degrees = curr_degrees;
00494     
00495     //scope.set(0, degrees);
00496     scope.set(0, speed);
00497     double speed_filtered = bqc.step(speed);
00498     scope.set(1,speed_filtered);
00499     scope.send();
00500 }
00501 */
00502 
00503 int main(){
00504     // Set baud connection with PC
00505     pc.baud(SERIAL_BAUD);
00506     pc.printf("\r\n ***THERMONUCLEAR WARFARE COMMENCES*** \r\n");
00507     
00508     // Setup Blinking LED
00509     led = 1;
00510     Ticker TickerBlinkLed;
00511     TickerBlinkLed.attach(BlinkLed,0.5);
00512     
00513     // Set motor PWM speeds
00514     motor1_pwm.period(1.0/1000);
00515     motor2_pwm.period(1.0/1000);
00516     
00517     // Setup Interruptin.fall
00518     button1.fall(button1_switch);
00519     button2.fall(Set_Mode);
00520     button3.fall(increase);
00521     button4.fall(decrease);
00522     // Setup motor control
00523     Ticker PIDControllerTicker;
00524     Ticker PotControlTicker;
00525     
00526     pc.printf("\r\n ***************** \r\n");
00527     pc.printf("\r\n Press button SW3 to start positioning the arms using PotControl\r\n");
00528     pc.printf("\r\n Make sure both potentiometers are positioned halfway! \r\n");
00529     pc.printf("\r\n ***************** \r\n");
00530     while (button1_value == false){}
00531     PotControlTicker.attach(&PotControl,0.01f);
00532     
00533     pc.printf("\r\n ***************** \r\n");
00534     pc.printf("\r\n When done positioning, press button SW3 to detach Potcontrol");
00535     pc.printf("\r\n ***************** \r\n");
00536     while (button1_value == true){}
00537     
00538     // Detach potcontrol
00539     PotControlTicker.detach();
00540     
00541     // Set motors to 0
00542     motor2_pwm.write(0);
00543     motor2_pwm.write(0);
00544     
00545     // Reset the encoders to set the 0 position
00546     m1_EncoderCW.reset();
00547     m1_EncoderCCW.reset();
00548     m2_EncoderCW.reset();
00549     m2_EncoderCCW.reset();
00550     
00551     pc.printf("\r\n ***************** \r\n");
00552     pc.printf("\r\n When done positioning, press button SW3 to start potmeter PID control");
00553     pc.printf("\r\n Make sure both potentiometers are positioned halfway!!! \r\n");
00554     pc.printf("\r\n ***************** \r\n");
00555     while (button1_value == false){}
00556     PIDControllerTicker.attach(&Controller_motor, CONTROLLER_TS); // 100 Hz
00557     while(true){}
00558 }