Luke Plummer
/
JesusLizard
lizard leg trajectory pd control code for 2.s994
Diff: main.cpp
- Revision:
- 3:6f00e4876cab
- Parent:
- 2:ea7ef710a07f
- Child:
- 4:af5440b3af19
--- a/main.cpp Mon Nov 25 20:17:48 2013 +0000 +++ b/main.cpp Mon Nov 25 21:54:30 2013 +0000 @@ -16,7 +16,7 @@ DigitalOut mDir2_B(p12); AnalogIn aIn2(p20); PwmOut pwmOut2(p22); -QEI encoder2(p25, p26, NC, 1200, QEI::X4_ENCODING); +QEI encoder2(p9, p10, NC, 1200, QEI::X4_ENCODING); // Declare other objects Ticker ctrlTicker; // creates an instance of the ticker class, which can be used for running functions at a specified frequency. @@ -32,18 +32,24 @@ float fPWM = 1000; int nTraj = 0; //trajectory index float aD[2] = {0.0, 0.0}; -float traj1[] = {}; -float traj2[] = {}; +//knee +float traj2[] = {0.0000,0.082808,0.16684,0.25007,0.3309,0.40823,0.48137,0.54996,0.61388,0.67319,0.72803,0.77865,0.82528,0.8682,0.90766,0.94392,0.9772,1.0077,1.0357,1.0613,1.0847,1.106,1.1253,1.1429,1.1587,1.1729,1.1856,1.1968,1.2066,1.215,1.2222,1.2281,1.2329,1.2364,1.2388,1.2401,1.2403,1.2395,1.2375,1.2346,1.2306,1.2256,1.2196,1.2126,1.2047,1.1958,1.1859,1.1751,1.1634,1.1508,1.1373,1.1229,1.1077,1.0917,1.0749,1.0573,1.039,1.02,1.0002,0.97986,0.95884,0.93722,0.91503,0.8923,0.86905,0.84532,0.82113,0.79652,0.77151,0.74614,0.72043,0.69442,0.66812,0.64158,0.61483,0.58788,0.56079,0.53357,0.50627,0.47892,0.45155,0.42421,0.39695,0.3698,0.34282,0.31606,0.28958,0.26345,0.23774,0.21252,0.18788,0.16391,0.14073,0.11845,0.097185,0.077089,0.058313,0.041026,0.02541,0.011664,0.0}; +//float traj2[] = {0.0,0.0,0.0}; +//hip +float traj1[] = {0,-0.07531,-0.1448,-0.20762,-0.2633,-0.31173,-0.35308,-0.38772,-0.41614,-0.43889,-0.45652,-0.4696,-0.47862,-0.48407,-0.48638,-0.48592,-0.48306,-0.47809,-0.47128,-0.46289,-0.45311,-0.44214,-0.43016,-0.41731,-0.40372,-0.38952,-0.37482,-0.35971,-0.3443,-0.32866,-0.31286,-0.29698,-0.28107,-0.26521,-0.24944,-0.23382,-0.21839,-0.2032,-0.18828,-0.17367,-0.15942,-0.14554,-0.13206,-0.11902,-0.10642,-0.0943,-0.08266,-0.07153,-0.0609,-0.05078,-0.04118,-0.03211,-0.02355,-0.0155,-0.00796,-0.00092,0.00563,0.0117,0.01731,0.02247,0.02719,0.0315,0.03542,0.03894,0.04211,0.04493,0.04742,0.04961,0.05151,0.05313,0.0545,0.05563,0.05654,0.05725,0.05776,0.05809,0.05825,0.05826,0.05813,0.05785,0.05744,0.0569,0.05623,0.05543,0.05449,0.05342,0.05218,0.05078,0.04919,0.04738,0.04533,0.043,0.04035,0.03732,0.03386,0.02991,0.0254,0.02024,0.01434,0.00763,0}; +//float traj1[] = {0.0,0.0,0.0}; int numPoints = sizeof(traj1)/sizeof(traj1[0]); -float fTraj = numPoints/3.0; //time frequency of trajectory commands +float fTraj = numPoints/0.5; //time frequency of trajectory commands //starting motor positions -float a0_1 = traj1[0]; -float a0_2 = traj2[0]; +float a1_0 = traj1[0]; +float a2_0 = traj2[0]; float e1 = 0; float e2 = 0; //controller gains -float kp = 0.7; -float kd = 0.0; +float kp1 = 1.7; +float kd1 = 0.0; +float kp2 = 1.5; +float kd2 = 0.0; bool done = false; @@ -67,21 +73,21 @@ //command motor speed pwmOut1.period(.0001); //set pwm frequency to 10kHz pwmOut2.period(.0001); //set pwm frequency to 10kHz - pwmOut1.write(abs(kp*e1)+abs(w1*kd)); - pwmOut2.write(abs(kp*e2)+abs(w2*kd)); + 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; + if (nTraj >= numPoints){ + nTraj = 0; + //done = true; } else { done = false; - aD[0] = traj1[nTraj]-a0_1; - aD[1] = traj2[nTraj]-a0_2; + 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++; } @@ -101,7 +107,8 @@ if (sizeof(traj1) == sizeof(traj2)) { trajTicker.attach(setTraj, 1/fTraj); ctrlTicker.attach(pdControl, 1/fPWM); - while (!done) {mySerial.printf("d: %f a: %f \n\r", aD[0], a1_t1);} + //while (!done) {mySerial.printf("targets 1: %f 2: %f \n\r", aD[0], aD[1]);} + while (!done) {mySerial.printf("motor 2: %f \n\r", a2_t1);} mySerial.printf("Done\n\r"); mDir1_A = 0; mDir1_B = 0;