Luke Plummer
/
JesusLizard
lizard leg trajectory pd control code for 2.s994
main.cpp
- Committer:
- lukeplummer
- Date:
- 2013-12-01
- Revision:
- 9:80c1f8bbcd55
- Parent:
- 8:448376e7d8f1
- Child:
- 10:6ecef8d1a018
File content as of revision 9:80c1f8bbcd55:
// 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); PwmOut pwmOut1(p21); QEI encoder1(p23, p24, NC, 1200, QEI::X4_ENCODING); //Motor 2 DigitalOut mDir2_A(p11); DigitalOut mDir2_B(p12); PwmOut pwmOut2(p22); QEI encoder2(p9, p10, NC, 1200, QEI::X4_ENCODING); //force measurement pins AnalogIn aIn1(p15); AnalogIn aIn2(p16); // Declare other objects Ticker ctrlTicker; // creates an instance of the ticker class, which can be used for running functions at a specified frequency. Ticker trajTicker; Ticker writeTicker; Serial mySerial(USBTX, USBRX); // create a serial connection to the computer over the tx/rx pins LocalFileSystem local("local"); FILE *fp1 = fopen("/local/encoders.csv", "w"); FILE *fp2 = fopen("/local/force.csv", "w"); DigitalOut dOut1(p17); 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 w1 = 0; float w2 = 0; float fPWM = 1000; int nTraj = 0; //trajectory index float aD[2] = {0.0, 0.0}; //knee float traj2[] = {0.0}; //hip float traj1[] = {0.0}; int numPoints = sizeof(traj1)/sizeof(traj1[0]); float fTraj = numPoints/0.5; //time frequency of trajectory commands //starting motor positions float a1_0 = traj1[0]; float a2_0 = traj2[0]; float e1 = 0; float e2 = 0; //controller gains float kp1 = 1.7; float kd1 = 0.0; float kp2 = 1.5; float kd2 = 0.0; bool done = false; void pdControl() { float in1 = aD[0]; float in2 = aD[1]; //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 w1 = abs(a1_t1-a1_t0)*fTraj; w2 = 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 pwmOut1.period(.0001); //set pwm frequency to 10kHz pwmOut2.period(.0001); //set pwm frequency to 10kHz pwmOut1.write(abs(kp1*e1)+abs(w1*kd1)); pwmOut2.write(abs(kp2*e2)+abs(w2*kd2)); a1_t0 = a1_t1; //save encoder position for next step (to find angular velocity) a2_t0 = a2_t1; } void setTraj() { if (nTraj >= numPoints){ nTraj = 0; //done = true; } else { done = false; aD[0] = traj1[nTraj]-a1_0; aD[1] = traj2[nTraj]-a2_0; // aD[1] = traj2[nTraj]-a0_2+aD[0]; //corrects for coaxial motors nTraj++; } } void writeFiles() { float f1 = aIn1.read(); float f2 = aIn2.read(); fprintf(fp1, "%f, %f\n",a1_t1, a2_t1); fprintf(fp2, "%f, %f average: $f\n",f1,f2, (f1+f2)/2); } int main() { wait(5.0); mDir1_A = 1; mDir1_B = 0; mDir2_A = 1; mDir2_B = 0; mySerial.printf("numPoints: %d, fTraj: %f, fPWM: %f\n\r", numPoints, fTraj, fPWM); //get initial position dOut1.write(1); wait(0.1); dOut1.write(0); if (sizeof(traj1) == sizeof(traj2)) { trajTicker.attach(setTraj, 1/fTraj); ctrlTicker.attach(pdControl, 1/fPWM); //writeTicker.attach(writeFiles, 0.01); while (!done) { mySerial.printf("motor 2: %f \n\r", a2_t1); } mySerial.printf("Done\n\r"); fclose(fp1); fclose(fp2); mDir1_A = 0; mDir1_B = 0; mDir2_A = 0; mDir2_B = 0; } else { mySerial.printf("Input error\n\r"); } }