main.cpp

00001 #include "mbed.h"
00002 #include "FastPWM.h"
00003 #include "MODSERIAL.h"
00004 #include "QEI.h"
00005 #include "math.h"
00006 
00007 // --------------------------------------------------
00008 // ----------------- SET UP -------------------------
00009 QEI Encoder1(D11, D10, NC, 4200) ;  // Encoder motor 1, (pin 1A, pin 1B, counts/rev)
00010 QEI Encoder2(D9, D8, NC, 4200) ;    // Encoder motor 2, (pin 2A, pin 2B, counts/rev)
00011 DigitalOut directionM1(D4);
00012 DigitalOut directionM2(D7);
00013 FastPWM motor1_pwm(D5);
00014 FastPWM motor2_pwm(D6);
00015 MODSERIAL pc(USBTX, USBRX);
00016 DigitalOut ledr(LED_RED);
00017 DigitalOut ledg(LED_GREEN);
00018 DigitalOut ledb(LED_BLUE);
00019 DigitalIn directionx(SW3); //x direction switch button
00020 DigitalIn directiony(SW2); //y direction switch button
00021 DigitalIn buttonx(D2); //x EMG replacement 
00022 DigitalIn buttony(D3); //y EMG replacement
00023 
00024 // Tickers
00025 Ticker Demo;
00026 Ticker Write;
00027 
00028 // ---------------------------------------------------
00029 // ----------------- GLOBAL VARIABLES ----------------
00030 volatile int counts1; 
00031 volatile int counts2;
00032 volatile double theta1;
00033 volatile double theta2;
00034 const double  pi = 3.14159265359;
00035 volatile double  error1;
00036 volatile double  error2;
00037 //volatile double  error1_final=10.0;
00038 //volatile double  error2_final=10.0;
00039 //volatile double q1_diff_final;
00040 //volatile double q2_diff_final;
00041 //double  point1x = 200.0;
00042 //double  point1y = 200.0;
00043 //double  point2x = 200.0;
00044 //double  point2y = 100.0;
00045 //double  point3x = 350.0;
00046 //double  point3y = 0.0;
00047 //double  point4x = 200.0;
00048 //double  point4y = 0.0;
00049 //volatile int track = 1;
00050 const double x0 = 80.0; //zero x position after homing
00051 const double y0 = 141.0; //zero y position after homing
00052 volatile double  setpointx = x0;
00053 volatile double  setpointy = y0;
00054 volatile double  U1;
00055 volatile double  U2;
00056 volatile double xfinal;
00057 volatile double yfinal;
00058 volatile int sx;//value of the button and store as switch
00059 volatile int sy;//value of the button and store as switch
00060 double dirx = 1.0; //determine the direction of the setpoint placement
00061 double diry = 1.0; //determine the direction of the setpoint placement
00062 
00063 const double v=0.1; //moving speed of setpoint 
00064 
00065 
00066 // Inverse Kinematics
00067 volatile double q1_diff;
00068 volatile double q2_diff;
00069 const double sq = 2.0; //to square numbers
00070 const double L1 = 250.0; //length of the first link
00071 const double L3 = 350.0; //length of the second link
00072 
00073 // Reference angles of the starting position
00074 double q2_0 = -acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3));
00075 double q1_0 = atan(y0/x0)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x0,sq)+pow(y0,sq))/(2.0*L1*sqrt(pow(x0,sq)+pow(y0,sq))));
00076 double q2_0_enc = (pi-q2_0) - q1_0;
00077 
00078 // --------------------------------------------------------------------
00079 // ---------------Read out encoders------------------------------------
00080 // --------------------------------------------------------------------    
00081 double  counts2angle1()
00082 {
00083     counts1 = Encoder1.getPulses();         // Counts of outputshaft of motor 1
00084     theta1 = -(double(counts1)/4200) * 2*pi;       // Angle of outputshaft of motor 1
00085     return theta1;
00086 }
00087 
00088 double counts2angle2()
00089 {
00090     counts2 = Encoder2.getPulses();         // Counts of outputshaft of motor 2
00091     theta2 = (double(counts2)/4200) * 2*pi;       // Angle of outputshaft of motor 2
00092     return theta2;
00093 }
00094 
00095 // -------------------------------------------------------------------------
00096 // -------------- Determine Setpoints --------------------------------------
00097 // -------------------------------------------------------------------------
00098 
00099 void determinedemoset(){
00100     setpointx = setpointx + dirx*sx*v;
00101     setpointy = setpointy + diry*sy*v;
00102     }
00103 
00104 //function to change the moving direction of the setpoint
00105 void ChangeDirectionX(){
00106     dirx = -1*dirx;
00107     }
00108     
00109 void ChangeDirectionY(){
00110     diry = -1*diry;
00111     }    
00112 
00113 
00114 
00115 // -----------------------------------------------------------------    
00116 // --------------------------- PI controllers ----------------------
00117 // -----------------------------------------------------------------
00118 double PI_controller1(double error1)
00119 {
00120     static double error_integral1 = 0;
00121     
00122     // Proportional part
00123     double Kp1 = 3.95;              // Kp (proportionele controller, nu nog een random waarde)
00124     double u_p1 = Kp1*error1;        // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
00125     
00126     // Integral part
00127     double Ki1 = 6.0;             // Ki (Integrale deel vd controller, nu nog een random waarde)
00128     double Ts1 = 0.005;           // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)   
00129     error_integral1 = error_integral1 + error1 * Ts1;  
00130     double u_i1 = Ki1 * error_integral1;
00131     
00132     // Sum 
00133     U1 = u_p1 + u_i1;
00134     
00135     // Return
00136     return U1;      
00137 }
00138 double PI_controller2(double error2)
00139 {
00140     static double error_integral2 = 0;
00141     
00142     // Proportional part
00143     double Kp2 = 3.95;              // Kp (proportionele controller, nu nog een random waarde)
00144     double u_p2 = Kp2*error2;        // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
00145     
00146     // Integral part
00147     double Ki2 = 6.0;             // Ki (Integrale deel vd controller, nu nog een random waarde)
00148     double Ts2 = 0.005;           // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)   
00149     error_integral2 = error_integral2 + error2 * Ts2;  
00150     double u_i2 = Ki2 * error_integral2;
00151     
00152     // Sum +
00153     U2 = u_p2 + u_i2;
00154     
00155     // Return
00156     return U2;      
00157 } 
00158 
00159 // ------------------------------------------------------------
00160 // ------------ Inverse Kinematics ----------------------------
00161 // ------------------------------------------------------------
00162 double makeAngleq1(double x, double y){
00163     double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
00164     q1_diff = -(2.0*(q1-q1_0));                                                                                    //the actual amount of radians that the motor has to turn in total to reach the setpoint
00165     return q1_diff;
00166 }
00167 
00168 double makeAngleq2(double x, double y){
00169     double q2 = -acos((pow(x,sq)+pow(y,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3)); //angle of the second joint in setpoint configuration
00170     double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
00171     double q2_motor = (pi - q2) - q1; //because q2 represents the angle at joint two and not at the motor a calculation has to be done
00172     q2_diff = (2.0*(q2_motor - q2_0_enc)); //the actual amount of radians that the motor has to turn in total to reach the setpoint
00173     return q2_diff;
00174 }
00175 
00176 
00177 // -----------------------------------------------
00178 // ------------ RUN MOTORS -----------------------
00179 // -----------------------------------------------
00180 void motoraansturing()
00181 {
00182     determinedemoset();
00183     q1_diff = makeAngleq1(setpointx, setpointy);
00184     q2_diff = makeAngleq2(setpointx, setpointy);
00185     //q1_diff_final = makeAngleq1(xfinal, yfinal);
00186     //q2_diff_final = makeAngleq2(xfinal, yfinal);
00187 
00188     theta2 = counts2angle2();           
00189     error2 = q2_diff - theta2;
00190     theta1 = counts2angle1();  
00191     error1 = q1_diff - theta1;          // Setpoint error, te behalen setpoint minus de huidige positie van de as.
00192     
00193     //errors die de setpoints bepalen
00194     //error1_final = q1_diff_final - theta1;
00195     //error2_final = q2_diff_final - theta2;
00196 
00197     U1 = PI_controller1(error1);       // Voltage dat naar de motor gestuurd wordt. 
00198     U2 = PI_controller2(error2);   
00199     
00200     motor1_pwm.write(fabs(U1));         // Motor aansturen
00201     directionM1 = U1 > 0.0f;            // Richting van de motor bepalen
00202     motor2_pwm.write(fabs(U2));
00203     directionM2 = U2 > 0.0f;     
00204 }
00205 
00206 
00207 void rundemo()
00208 {
00209     motoraansturing();
00210 }
00211 
00212 
00213 int main()
00214 {
00215     pc.baud(115200);
00216     motor1_pwm.period_us(60);                // Period is 60 microseconde
00217     motor2_pwm.period_us(60);
00218     Demo.attach(&rundemo, 0.005f); 
00219     
00220     InterruptIn directionx(SW3);
00221     directionx.fall(ChangeDirectionX); //change the direction of the setpoint in x direction
00222     InterruptIn directiony(SW2);
00223     directiony.fall(ChangeDirectionY); //change the direction of the setpoint in y direction
00224     
00225     pc.printf("\r\n\r\nDOE HET AUB!!! \r\n\r\n");
00226     
00227     while (true) {
00228         sx = !buttonx.read(); //this has to be replaced with the input from the EMG, this then functions like a button
00229         sy = !buttony.read(); //this has to be replaced with the input from the EMG, this then functions like a button
00230         pc.printf("Setpointx: %0.2f, Setpointy: %02f, q1_diff: %0.2f, q2_diff: %0.2f, error1: %0.2f, error2: %0.2f, U1: %0.2f, U2: %0.2f\r\n", setpointx,setpointy,q1_diff,q2_diff,error1,error2,U1,U2);
00231         //if (track == 1) {
00232             //pc.printf("Gaat naar positie 1\r\n");
00233         //}
00234         //else if (track == 12) {
00235             //pc.printf("Gaat naar positie 2\r\n");
00236         //}
00237         wait(0.5f);
00238    
00239     }
00240 }