main.cpp

00001 //libaries
00002 #include "mbed.h"
00003 #include "encoder.h"
00004 #include "MODSERIAL.h"
00005 
00006 // globale variables
00007 Ticker Treecko;                 //We make a awesome ticker for our control system
00008 const float Ts = 0.1;                   // tickettijd/ sample time
00009 MODSERIAL pc(USBTX,USBRX);
00010 float PwmPeriod = 1.0/5000.0;   //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
00011 AnalogIn potMeter2(A1);         //Analoge input of potmeter 2 (will be use for te reference position)
00012 AnalogIn potMeter1(A2);
00013 DigitalIn Knopje1 (PTA4);       //tijdelijk
00014 DigitalIn Knopje2 (PTC6);       //tijdelijk
00015 bool autodemo_done = 1;         //automatische demo stand =0
00016 
00017 //eerste motor
00018 PwmOut M1E(D6);             //Biorobotics Motor 1 PWM control of the speed 
00019 DigitalOut M1D(D7);         //Biorobotics Motor 1 diraction control
00020 Encoder motor1(D13,D12,true);
00021 
00022 float e_prev = 0; 
00023 float e_int = 0;
00024 float error1;
00025 
00026 //tweede motor
00027 PwmOut M2E(D5);
00028 DigitalOut M2D(D4);
00029 Encoder motor2(D9,D8,true);
00030 
00031 
00032 float e_prev2 = 0; 
00033 float e_int2 = 0;
00034 float error2;
00035 
00036 //RKI
00037 double pi = 3.14159265359;
00038 double q1 = (pi/2);    //Reference position hoek 1 in radiance
00039 double q2 = -(pi/2);   //Reference position hoek 2 in radiance
00040 const double L1 = 0.30;        //Length arm 1 in mm
00041 const double L2 = 0.38;        //Length arm 2 in mm
00042 double B1 = 1;          //Friction constant motor 1
00043 double B2 = 1;          //Friction constant motor 2
00044 double K = 1;           //Spring constant movement from end-effector position to setpoint position
00045 double Tijd = 1;           //Timestep value
00046 double Rsx = 0.38;       //Reference x-component of the setpoint radius
00047 double Rsy = 0.30;       //Reference y-component of the setpoint radius
00048 double Motor1Set = 0;      //Reference position motor 1
00049 double Motor2Set = 0.5*pi; //Reference position motor 2
00050 double Rex = cos(q1)*L1 - sin(q2)*L2;   //The x-component of the end-effector radius 
00051 double Rey = sin(q1)*L1 + cos(q2)*L2;   //The y-component of the end-effector radius
00052 double R1x = 0;                         //The x-component of the joint 1 radius
00053 double R1y = 0;                         //The y-component of the joint 1 radius
00054 double R2x = cos(q1)*L1;                //The x-component of the joint 2 radius
00055 double R2y = sin(q1)*L1;                //The y-component of the joint 1 radius   
00056 double Fx = 0;
00057 double Fy = 0;
00058 double Tor1 = 0;
00059 double Tor2 = 0;
00060 double w1=0;
00061 double w2=0;
00062 
00063 void RKI()
00064 {
00065     Rex = cos(q1)*L1 - sin(q2)*L2;
00066     Rey = sin(q1)*L1 + cos(q2)*L2;
00067     R2x = cos(q1)*L1;
00068     R2y = sin(q1)*L1;
00069     Fx = (Rsx-Rex)*K;
00070     Fy = (Rsy-Rey)*K;
00071     Tor1 = (Rex-R1x)*Fy + (R1y-Rey)*Fx;
00072     Tor2 = (Rex-R2x)*Fy + (R2y-Rey)*Fx;
00073     w1 = Tor1/B1;
00074     w2 = Tor2/B2;
00075     q1 = q1 + w1*Tijd;
00076     q2 = q2 + w2*Tijd;
00077     
00078     
00079     /*if (tau1 < 0.00001){
00080       tau1 = 0;
00081     }
00082     else {
00083         tau1 = tau1;}
00084     if (tau2 < 0.00001){
00085       tau2 = 0;
00086     }
00087     else{
00088         tau2 = tau2;}*/
00089     
00090     int maxwaarde = 4096;                   // = 64x64
00091     Motor2Set = (((0.5*pi) + q1 - q2)/(2*pi))*maxwaarde;
00092     Motor1Set = (((0.5*pi) - q1)/(2*pi))*maxwaarde;
00093     
00094     //Motor1Set = -(q1/(2*pi))*maxwaarde;           //Calculate the desired motor1 angle from the desired joint positions
00095     //Motor2Set = ((pi-q2-q1)/(2*pi))*maxwaarde;   //Calculate the desired motor2 angle from the desired joint positions
00096 
00097 }
00098 
00099 void SetpointRobot()
00100 {   
00101     //double Potmeterwaarde2 = potMeter2.read();
00102     //double Potmeterwaarde1 = potMeter1.read();
00103     bool knop1 = Knopje1;
00104     bool knop2 = Knopje2;
00105 
00106     if  (knop1 == false){        //(Potmeterwaarde2>0.6) {
00107         Rsx += 0.001;    // hoe veel verder gaat hij? 1 cm? 10 cm?
00108     }
00109     else if  (knop1 == true){  //(Potmeterwaarde2<0.4) {
00110                                 //SetPx -= 0.001;
00111     }
00112     /*else
00113     {}*/
00114     if (knop2 == false){         //(Potmeterwaarde1>0.6) {
00115         Rsy += 0.001;
00116     }
00117     else if (knop2 ==true){    //(Potmeterwaarde1<0.4) {
00118                                 //SetPy -= 0.001;
00119     }
00120     /*else
00121     {}*/
00122     //pc.printf("Setpointx = %f, Setpointy = %f \r\n", SetPx, SetPy);
00123 }
00124 
00125 /*float GetReferencePosition() 
00126 {
00127     float Potmeterwaarde = potMeter2.read();
00128     int maxwaarde = 4096;                   // = 64x64
00129     float refP = Potmeterwaarde*maxwaarde;
00130     return refP;                            // value between 0 and 4096 
00131 }
00132 
00133 float GetReferencePosition2() 
00134 {
00135     float Potmeterwaarde2 = potMeter1.read();
00136     int maxwaarde2 = 4096;                   // = 64x64
00137     float refP2 = Potmeterwaarde2*maxwaarde2;
00138     return refP2;                            // value between 0 and 4096 
00139 }*/
00140     /*
00141 float FeedBackControl(float error, float &e_prev, float &e_int)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
00142 {
00143     float kp = 0.0005;                             // kind of scaled.
00144     float Proportional= kp*error;
00145     
00146     float kd = 0.0004;                           // kind of scaled. 
00147     float VelocityError = (error - e_prev)/Ts; 
00148     float Derivative = kd*VelocityError;
00149     e_prev = error;
00150     
00151     float ki = 0.00005;                           // kind of scaled.
00152     e_int = e_int+Ts*error;
00153     float Integrator = ki*e_int;
00154     
00155     
00156     float motorValue = Proportional + Integrator + Derivative;
00157     return motorValue;
00158 }
00159 
00160 float FeedBackControl2(float error2, float &e_prev2, float &e_int2)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
00161 {
00162     float kp2 = 0.0005;                             // kind of scaled.
00163     float Proportional2= kp2*error2;
00164     
00165     float kd2 = 0.0004;                           // kind of scaled. 
00166     float VelocityError2 = (error2 - e_prev2)/Ts; 
00167     float Derivative2 = kd2*VelocityError2;
00168     e_prev2 = error2;
00169     
00170     float ki2 = 0.00005;                           // kind of scaled.
00171     e_int2 = e_int2+Ts*error2;
00172     float Integrator2 = ki2*e_int2;
00173     
00174     
00175     float motorValue2 = Proportional2 + Integrator2 + Derivative2;
00176     return motorValue2;
00177 }
00178 
00179 
00180 void SetMotor1(float motorValue)
00181 {
00182     if (motorValue >= 0)
00183     {
00184         M1D = 0;                    //direction ...
00185     }
00186     else 
00187     {
00188         M1D = 1;                    //direction ...
00189     }
00190 
00191     if  (fabs(motorValue) > 1)    
00192     {
00193         M1E = 1;                    //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
00194     }
00195     else
00196     {    
00197         M1E = fabs(motorValue);      //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
00198     }
00199 }
00200 
00201 void SetMotor2(float motorValue2)
00202 {
00203     if (motorValue2 >= 0)
00204     {
00205         M2D = 1; //PAS OP, DEZE MOET ZO ZIJN!
00206     }
00207     else 
00208     {
00209         M2D = 0;
00210     }
00211 
00212     if  (fabs(motorValue2) > 1)    
00213     {
00214         M2E = 1;                    //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
00215     }
00216     else
00217     {    
00218         M2E = fabs(motorValue2);      //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
00219     }
00220 }
00221 
00222 float Encoder ()
00223 {
00224     float Huidigepositie = motor1.getPosition ();
00225     return Huidigepositie;             // huidige positie = current position
00226 }
00227 
00228 float Encoder2 ()
00229 {
00230     float Huidigepositie2 = motor2.getPosition ();
00231     return Huidigepositie2;             // huidige positie = current position
00232 }
00233 */
00234 void MeasureAndControl(void)
00235 {
00236     SetpointRobot(); 
00237     // RKI aanroepen
00238     RKI();
00239     /*
00240     // hier the control of the control system
00241     //float refP = GetReferencePosition(); 
00242     float Huidigepositie = Encoder(); 
00243     error1 = (Motor1Set - Huidigepositie);// make an error
00244     float motorValue = FeedBackControl(error1, e_prev, e_int);
00245     SetMotor1(motorValue);
00246 
00247     // hier the control of the control system
00248     //float refP2 = GetReferencePosition2(); 
00249     float Huidigepositie2 = Encoder2(); 
00250     error2 = (Motor2Set - Huidigepositie2);// make an error
00251     float motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
00252     SetMotor2(motorValue2);*/
00253 }
00254 /*
00255 void Autodemo_or_demo()
00256 {
00257     if (autodemo_done == 0)
00258     {
00259     SetPx = 0.38 + 0.15;
00260     SetPy = 0.30; 
00261     MeasureAndControl ();
00262     SetPx = 0.38;
00263     SetPy = 0.30; 
00264     MeasureAndControl ();   
00265     SetPx = 0.38;
00266     SetPy = 0.30 - 0.30; 
00267     MeasureAndControl ();
00268     SetPx = 0.38;
00269     SetPy = 0.30; 
00270     MeasureAndControl ();
00271     autodemo_done = 1;
00272     }
00273         
00274     else if (autodemo_done == 1)
00275     {
00276     SetpointRobot();
00277     MeasureAndControl ();
00278     }
00279     
00280 }
00281 */
00282 
00283 int main()
00284 {
00285     M1E.period(PwmPeriod);
00286     Treecko.attach(&MeasureAndControl, Ts);   //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende 
00287                                             //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
00288     pc.baud(115200);
00289      
00290      
00291     while(1) 
00292     {
00293         //wait(0.2);
00294         //float B = motor1.getPosition();
00295         pc.printf("Setpointx = %f, Setpointy = %f, tau1 = %f, tau2 = %f, Motor1Set = %f, Motor2Set = %f,", Rsx, Rsy, Tor1, Tor2, Motor1Set, Motor2Set);
00296         pc.printf(" q1 = %f, q2 = %f \r\n", q1, q2);
00297         //float positie = B%4096;
00298         //pc.printf("pos: %d, speed %f, potmeter = %f V, \r\n",motor1.getPosition(), motor1.getSpeed(),(potMeter2.read()*3.3)); //potmeter uitlezen. tussen 0-1. voltage, dus *3.3V
00299         //pc.printf("q1 = %f, q2 = %f, Motor1Set = %f, Motor2Set = %f \r\n", q1, q2, Motor1Set, Motor2Set);   
00300     }
00301 }