Luke Plummer
/
JesusLizard
lizard leg trajectory pd control code for 2.s994
main.cpp
- Committer:
- lukeplummer
- Date:
- 2013-11-17
- Revision:
- 0:e6c3a3a00c16
- Child:
- 1:fa246c82ab54
File content as of revision 0:e6c3a3a00c16:
// THE FIGHTING BANANA SLUGS!!! #include "mbed.h" // mbed library #include "QEI.h" // quadrature encoder library to count encoder ticks //Setup //Motor 1 DigitalOut mDir1_A(p5); DigitalOut mDir1_B(p6) AnalogIn aIn1(p19); PwmOut pwmOut1(p21); QEI encoder1(p23, p24, NC, 1200, QEI::X4_ENCODING); //Motor 2 DigitalOut mDir2_A(p11); DigitalOut mDir2_B(p12); AnalogIn aIn2(p20); PwmOut pwmOut2(p22); QEI encoder2(p25, p26, NC, 1200, QEI::X4_ENCODING); high1 = 1; low1 = 0; high2 = 1; low2 = 0; // Declare other objects Ticker trajTicker; Ticker ctrlTicker; // creates an instance of the ticker class, which can be used for running functions at a specified frequency. Serial mySerial(USBTX, USBRX); // create a serial connection to the computer over the tx/rx pins float a1_t0 = 0; //motor angle 1 from previous time step float a1_t1 = 0; //motor angle 1 from current time step float a2_t0 = 0; //motor angle 2 from current time step float a2_t1 = 0; //motor angle 2 from current time step float omega1 = 0; float omega2 = 0; float fTraj = 1.0; //time frequency of trajectory commands int nTraj = 0; //trajectory index float Ad1[10] = {0}; //target angles float Ad2[10] = {0}; float e1 = 0; float e2 = 0; float kp = 1; float kd = 1; void getTraj(i) { } void pdControl(float in1, float in2) { //get motor position a1_t1 = encoder1.getPulses()*2*3.14/1200.0; a2_t1 = encoder2.getPulses()*2*3.14/1200.0; //calculate error e1 = a1_t1-in1; e2 = a2_t1-in2; //calculate motor speed omega1 = abs(a1_t1-a1_t0)*fTraj; omega2 = abs(a2_t1-a2_t0)*fTraj; //set motor direction mDir1_A = (e1<0); mDir1_B = !(e1<0); mDir2_A = (e2>0); mDir2_B = !(e2>0); //command motor speed pwmOut.period(.0001); //set pwm frequency to 10kHz pwmOut1.write(abs(kp*e1)+abs(w1*kd)); pwmOut2.write(abs(kp*e2)+abs(w2*kd)); mySerial.printf("Angles: %f, %f \n\r", a2_t1, a1_t1); a1_t0 = a1_t1; //save encoder position for next step (to find angular velocity) a2_t0 = a2_t1; } int main() { if (sizeOf(aD1) == sizeOf(aD2)) { } } else { mySerial.printf("Input error"); } }