Luke Plummer / Mbed 2 deprecated JesusLizard

lizard leg trajectory pd control code for 2.s994

Dependencies:   QEI mbed

main.cpp@7:e64cd99abefb, 2013-12-01 (annotated)

Committer:
lukeplummer
Date:
Sun Dec 01 19:37:53 2013 +0000
Revision:
7:e64cd99abefb
Parent:
6:8cd6aa47fd51
Child:
8:448376e7d8f1
added force logging;

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