Luke Plummer / Mbed 2 deprecated JesusLizard

lizard leg trajectory pd control code for 2.s994

Dependencies:   QEI mbed

main.cpp@4:af5440b3af19, 2013-12-01 (annotated)

Committer:
lukeplummer
Date:
Sun Dec 01 17:33:41 2013 +0000
Revision:
4:af5440b3af19
Parent:
3:6f00e4876cab
Child:
5:9f31c55179d7
added filewriting

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lukeplummer 0:e6c3a3a00c16 1 // THE FIGHTING BANANA SLUGS!!!
lukeplummer 0:e6c3a3a00c16 2
lukeplummer 0:e6c3a3a00c16 3 #include "mbed.h" // mbed library
lukeplummer 0:e6c3a3a00c16 4 #include "QEI.h" // quadrature encoder library to count encoder ticks
lukeplummer 0:e6c3a3a00c16 5
lukeplummer 0:e6c3a3a00c16 6 //Setup
lukeplummer 0:e6c3a3a00c16 7 //Motor 1
lukeplummer 0:e6c3a3a00c16 8 DigitalOut mDir1_A(p5);
lukeplummer 1:fa246c82ab54 9 DigitalOut mDir1_B(p6);
lukeplummer 0:e6c3a3a00c16 10 AnalogIn aIn1(p19);
lukeplummer 0:e6c3a3a00c16 11 PwmOut pwmOut1(p21);
lukeplummer 0:e6c3a3a00c16 12 QEI encoder1(p23, p24, NC, 1200, QEI::X4_ENCODING);
lukeplummer 0:e6c3a3a00c16 13
lukeplummer 0:e6c3a3a00c16 14 //Motor 2
lukeplummer 0:e6c3a3a00c16 15 DigitalOut mDir2_A(p11);
lukeplummer 0:e6c3a3a00c16 16 DigitalOut mDir2_B(p12);
lukeplummer 0:e6c3a3a00c16 17 AnalogIn aIn2(p20);
lukeplummer 0:e6c3a3a00c16 18 PwmOut pwmOut2(p22);
lukeplummer 3:6f00e4876cab 19 QEI encoder2(p9, p10, NC, 1200, QEI::X4_ENCODING);
lukeplummer 0:e6c3a3a00c16 20
lukeplummer 0:e6c3a3a00c16 21 // Declare other objects
lukeplummer 1:fa246c82ab54 22 Ticker ctrlTicker; // creates an instance of the ticker class, which can be used for running functions at a specified frequency.
lukeplummer 0:e6c3a3a00c16 23 Ticker trajTicker;
lukeplummer 0:e6c3a3a00c16 24 Serial mySerial(USBTX, USBRX); // create a serial connection to the computer over the tx/rx pins
lukeplummer 4:af5440b3af19 25 LocalFileSystem local("local");
lukeplummer 4:af5440b3af19 26 FILE *fp = fopen("/local/out.csv", "w");
lukeplummer 0:e6c3a3a00c16 27
lukeplummer 0:e6c3a3a00c16 28 float a1_t0 = 0; //motor angle 1 from previous time step
lukeplummer 0:e6c3a3a00c16 29 float a1_t1 = 0; //motor angle 1 from current time step
lukeplummer 0:e6c3a3a00c16 30 float a2_t0 = 0; //motor angle 2 from current time step
lukeplummer 0:e6c3a3a00c16 31 float a2_t1 = 0; //motor angle 2 from current time step
lukeplummer 1:fa246c82ab54 32 float w1 = 0;
lukeplummer 1:fa246c82ab54 33 float w2 = 0;
lukeplummer 2:ea7ef710a07f 34 float fPWM = 1000;
lukeplummer 0:e6c3a3a00c16 35 int nTraj = 0; //trajectory index
lukeplummer 2:ea7ef710a07f 36 float aD[2] = {0.0, 0.0};
lukeplummer 3:6f00e4876cab 37 //knee
lukeplummer 3:6f00e4876cab 38 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};
lukeplummer 3:6f00e4876cab 39 //float traj2[] = {0.0,0.0,0.0};
lukeplummer 3:6f00e4876cab 40 //hip
lukeplummer 3:6f00e4876cab 41 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};
lukeplummer 3:6f00e4876cab 42 //float traj1[] = {0.0,0.0,0.0};
lukeplummer 2:ea7ef710a07f 43 int numPoints = sizeof(traj1)/sizeof(traj1[0]);
lukeplummer 3:6f00e4876cab 44 float fTraj = numPoints/0.5; //time frequency of trajectory commands
lukeplummer 2:ea7ef710a07f 45 //starting motor positions
lukeplummer 3:6f00e4876cab 46 float a1_0 = traj1[0];
lukeplummer 3:6f00e4876cab 47 float a2_0 = traj2[0];
lukeplummer 0:e6c3a3a00c16 48 float e1 = 0;
lukeplummer 0:e6c3a3a00c16 49 float e2 = 0;
lukeplummer 2:ea7ef710a07f 50 //controller gains
lukeplummer 3:6f00e4876cab 51 float kp1 = 1.7;
lukeplummer 3:6f00e4876cab 52 float kd1 = 0.0;
lukeplummer 3:6f00e4876cab 53 float kp2 = 1.5;
lukeplummer 3:6f00e4876cab 54 float kd2 = 0.0;
lukeplummer 2:ea7ef710a07f 55
lukeplummer 1:fa246c82ab54 56 bool done = false;
lukeplummer 0:e6c3a3a00c16 57
lukeplummer 1:fa246c82ab54 58 void pdControl() {
lukeplummer 2:ea7ef710a07f 59 float in1 = aD[0];
lukeplummer 2:ea7ef710a07f 60 float in2 = aD[1];
lukeplummer 0:e6c3a3a00c16 61 //get motor position
lukeplummer 0:e6c3a3a00c16 62 a1_t1 = encoder1.getPulses()*2*3.14/1200.0;
lukeplummer 0:e6c3a3a00c16 63 a2_t1 = encoder2.getPulses()*2*3.14/1200.0;
lukeplummer 0:e6c3a3a00c16 64 //calculate error
lukeplummer 0:e6c3a3a00c16 65 e1 = a1_t1-in1;
lukeplummer 0:e6c3a3a00c16 66 e2 = a2_t1-in2;
lukeplummer 0:e6c3a3a00c16 67 //calculate motor speed
lukeplummer 1:fa246c82ab54 68 w1 = abs(a1_t1-a1_t0)*fTraj;
lukeplummer 1:fa246c82ab54 69 w2 = abs(a2_t1-a2_t0)*fTraj;
lukeplummer 0:e6c3a3a00c16 70 //set motor direction
lukeplummer 0:e6c3a3a00c16 71 mDir1_A = (e1<0);
lukeplummer 0:e6c3a3a00c16 72 mDir1_B = !(e1<0);
lukeplummer 0:e6c3a3a00c16 73 mDir2_A = (e2>0);
lukeplummer 0:e6c3a3a00c16 74 mDir2_B = !(e2>0);
lukeplummer 0:e6c3a3a00c16 75 //command motor speed
lukeplummer 1:fa246c82ab54 76 pwmOut1.period(.0001); //set pwm frequency to 10kHz
lukeplummer 1:fa246c82ab54 77 pwmOut2.period(.0001); //set pwm frequency to 10kHz
lukeplummer 3:6f00e4876cab 78 pwmOut1.write(abs(kp1*e1)+abs(w1*kd1));
lukeplummer 3:6f00e4876cab 79 pwmOut2.write(abs(kp2*e2)+abs(w2*kd2));
lukeplummer 0:e6c3a3a00c16 80
lukeplummer 0:e6c3a3a00c16 81 a1_t0 = a1_t1; //save encoder position for next step (to find angular velocity)
lukeplummer 0:e6c3a3a00c16 82 a2_t0 = a2_t1;
lukeplummer 0:e6c3a3a00c16 83 }
lukeplummer 0:e6c3a3a00c16 84
lukeplummer 1:fa246c82ab54 85 void setTraj() {
lukeplummer 3:6f00e4876cab 86 if (nTraj >= numPoints){
lukeplummer 3:6f00e4876cab 87 nTraj = 0;
lukeplummer 3:6f00e4876cab 88 //done = true;
lukeplummer 1:fa246c82ab54 89 } else {
lukeplummer 1:fa246c82ab54 90 done = false;
lukeplummer 3:6f00e4876cab 91 aD[0] = traj1[nTraj]-a1_0;
lukeplummer 3:6f00e4876cab 92 aD[1] = traj2[nTraj]-a2_0;
lukeplummer 2:ea7ef710a07f 93 // aD[1] = traj2[nTraj]-a0_2+aD[0]; //corrects for coaxial motors
lukeplummer 1:fa246c82ab54 94 nTraj++;
lukeplummer 1:fa246c82ab54 95 }
lukeplummer 1:fa246c82ab54 96 }
lukeplummer 1:fa246c82ab54 97
lukeplummer 4:af5440b3af19 98 void fileWriter(float in1, float in2) {
lukeplummer 4:af5440b3af19 99 n++;
lukeplummer 4:af5440b3af19 100 if(n<3000){
lukeplummer 4:af5440b3af19 101 fprintf(fp, "%f,%f\n",in1, in2);
lukeplummer 4:af5440b3af19 102 }
lukeplummer 4:af5440b3af19 103 else{
lukeplummer 4:af5440b3af19 104 fclose(fp);
lukeplummer 4:af5440b3af19 105 }
lukeplummer 4:af5440b3af19 106 }
lukeplummer 4:af5440b3af19 107
lukeplummer 0:e6c3a3a00c16 108 int main() {
lukeplummer 1:fa246c82ab54 109 mDir1_A = 1;
lukeplummer 1:fa246c82ab54 110 mDir1_B = 0;
lukeplummer 1:fa246c82ab54 111 mDir2_A = 1;
lukeplummer 1:fa246c82ab54 112 mDir2_B = 0;
lukeplummer 1:fa246c82ab54 113
lukeplummer 2:ea7ef710a07f 114 mySerial.printf("numPoints: %d, fTraj: %f, fPWM: %f\n\r", numPoints, fTraj, fPWM);
lukeplummer 2:ea7ef710a07f 115
lukeplummer 2:ea7ef710a07f 116 //get initial position
lukeplummer 2:ea7ef710a07f 117
lukeplummer 2:ea7ef710a07f 118
lukeplummer 1:fa246c82ab54 119 if (sizeof(traj1) == sizeof(traj2)) {
lukeplummer 1:fa246c82ab54 120 trajTicker.attach(setTraj, 1/fTraj);
lukeplummer 1:fa246c82ab54 121 ctrlTicker.attach(pdControl, 1/fPWM);
lukeplummer 3:6f00e4876cab 122 //while (!done) {mySerial.printf("targets 1: %f 2: %f \n\r", aD[0], aD[1]);}
lukeplummer 4:af5440b3af19 123 while (!done) {
lukeplummer 4:af5440b3af19 124 mySerial.printf("motor 2: %f \n\r", a2_t1);
lukeplummer 4:af5440b3af19 125 fprintf(fp, "%f, %f\n",a1_t1, a2_t1);
lukeplummer 4:af5440b3af19 126 }
lukeplummer 1:fa246c82ab54 127 mySerial.printf("Done\n\r");
lukeplummer 4:af5440b3af19 128 fclose(fp);
lukeplummer 1:fa246c82ab54 129 mDir1_A = 0;
lukeplummer 1:fa246c82ab54 130 mDir1_B = 0;
lukeplummer 1:fa246c82ab54 131 mDir2_A = 0;
lukeplummer 1:fa246c82ab54 132 mDir2_B = 0;
lukeplummer 0:e6c3a3a00c16 133 } else {
lukeplummer 1:fa246c82ab54 134 mySerial.printf("Input error\n\r");
lukeplummer 0:e6c3a3a00c16 135 }
lukeplummer 0:e6c3a3a00c16 136 }