main.cpp

00001 // THE FIGHTING BANANA SLUGS!!!
00002  
00003 #include "mbed.h"       // mbed library
00004 #include "QEI.h"        // quadrature encoder library to count encoder ticks
00005 
00006 //Setup
00007 //Motor 1
00008 DigitalOut mDir1_A(p5);
00009 DigitalOut mDir1_B(p6);
00010 PwmOut pwmOut1(p21);
00011 QEI encoder1(p23, p24, NC, 1200, QEI::X4_ENCODING);
00012 
00013 //Motor 2
00014 DigitalOut mDir2_A(p11);
00015 DigitalOut mDir2_B(p12);
00016 PwmOut pwmOut2(p22);
00017 QEI encoder2(p9, p10, NC, 1200, QEI::X4_ENCODING);
00018 
00019 DigitalIn button(p7);
00020 
00021 // Declare other objects
00022 Ticker ctrlTicker;                // creates an instance of the ticker class, which can be used for running functions at a specified frequency.
00023 Ticker trajTicker;
00024 Ticker buttonTicker;
00025 Serial mySerial(USBTX, USBRX);  // create a serial connection to the computer over the tx/rx pins
00026 
00027 DigitalOut dOut1(p17);
00028 
00029 float a1_t0 = 0;    //motor angle 1 from previous time step
00030 float a1_t1 = 0;    //motor angle 1 from current time step
00031 float a2_t0 = 0;    //motor angle 2 from current time step
00032 float a2_t1 = 0;    //motor angle 2 from current time step
00033 float w1 = 0;
00034 float w2 = 0;
00035 float fPWM = 1000;
00036 int nTraj = 0; //trajectory index
00037 float aD[2] = {0.0, 0.0};
00038 //knee
00039 float traj2[] = {0.0};
00040 //hip
00041 float traj1[] = {0.0};
00042 int numPoints = sizeof(traj1)/sizeof(traj1[0]);
00043 float fTraj = numPoints/0.5;   //time frequency of trajectory commands
00044 //starting motor positions
00045 float a1_0 = traj1[0];
00046 float a2_0 = traj2[0];
00047 float e1 = 0;
00048 float e2 = 0;
00049 //controller gains
00050 float kp1 = 1.7;
00051 float kd1 = 0.0;
00052 float kp2 = 1.5;
00053 float kd2 = 0.0;
00054 
00055 bool done = false;
00056 
00057 void pdControl() { 
00058     float in1 = aD[0];
00059     float in2 = aD[1];
00060     //get motor position
00061     a1_t1 = encoder1.getPulses()*2*3.14/1200.0;
00062     a2_t1 = encoder2.getPulses()*2*3.14/1200.0;
00063     //calculate error
00064     e1 = a1_t1-in1;
00065     e2 = a2_t1-in2;
00066     //calculate motor speed
00067     w1 = abs(a1_t1-a1_t0)*fTraj;
00068     w2 = abs(a2_t1-a2_t0)*fTraj;
00069     //set motor direction
00070     mDir1_A = (e1<0);
00071     mDir1_B = !(e1<0);
00072     mDir2_A = (e2>0);
00073     mDir2_B = !(e2>0);
00074     //command motor speed
00075     pwmOut1.period(.0001); //set pwm frequency to 10kHz
00076     pwmOut2.period(.0001); //set pwm frequency to 10kHz
00077     pwmOut1.write(abs(kp1*e1)+abs(w1*kd1));
00078     pwmOut2.write(abs(kp2*e2)+abs(w2*kd2));
00079     
00080     a1_t0 = a1_t1; //save encoder position for next step (to find angular velocity)
00081     a2_t0 = a2_t1;
00082 }
00083 
00084 void setTraj() {
00085     if (nTraj >= numPoints){
00086         nTraj = 0;
00087         //done = true;
00088     } else {
00089         done = false;
00090         aD[0] = traj1[nTraj]-a1_0;
00091         aD[1] = traj2[nTraj]-a2_0;
00092         nTraj++;
00093     }
00094 }
00095 
00096 void checkBtn() {
00097     if (button.read() == 1){
00098         done = true;
00099     }
00100 }
00101 
00102 int main() {
00103     wait(5.0);
00104     mDir1_A = 1;
00105     mDir1_B = 0;
00106     mDir2_A = 1;
00107     mDir2_B = 0;
00108     
00109     //mySerial.printf("numPoints: %d, fTraj: %f, fPWM: %f\n\r", numPoints, fTraj, fPWM);
00110     
00111     //get initial position
00112     
00113     dOut1.write(1);
00114     
00115     if (sizeof(traj1) == sizeof(traj2)) {
00116         trajTicker.attach(setTraj, 1/fTraj);
00117         ctrlTicker.attach(pdControl, 1/fPWM);
00118         buttonTicker.attach(checkBtn, 0.1);
00119         while (!done) {
00120             //mySerial.printf("motor 2: %f \n\r", a2_t1);
00121         }
00122         //mySerial.printf("Done\n\r");
00123         dOut1.write(0);
00124         mDir1_A = 0;
00125         mDir1_B = 0;
00126         mDir2_A = 0;
00127         mDir2_B = 0;
00128     } else {
00129         //mySerial.printf("Input error\n\r");
00130     }
00131 }