Luke Plummer / Mbed 2 deprecated JesusLizard

lizard leg trajectory pd control code for 2.s994

Dependencies:   QEI mbed

main.cpp@2:ea7ef710a07f, 2013-11-25 (annotated)

Committer:
lukeplummer
Date:
Mon Nov 25 20:17:48 2013 +0000
Revision:
2:ea7ef710a07f
Parent:
1:fa246c82ab54
Child:
3:6f00e4876cab
2 motors, functional

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 0:e6c3a3a00c16 19 QEI encoder2(p25, p26, 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 0:e6c3a3a00c16 25
lukeplummer 0:e6c3a3a00c16 26 float a1_t0 = 0; //motor angle 1 from previous time step
lukeplummer 0:e6c3a3a00c16 27 float a1_t1 = 0; //motor angle 1 from current time step
lukeplummer 0:e6c3a3a00c16 28 float a2_t0 = 0; //motor angle 2 from current time step
lukeplummer 0:e6c3a3a00c16 29 float a2_t1 = 0; //motor angle 2 from current time step
lukeplummer 1:fa246c82ab54 30 float w1 = 0;
lukeplummer 1:fa246c82ab54 31 float w2 = 0;
lukeplummer 2:ea7ef710a07f 32 float fPWM = 1000;
lukeplummer 0:e6c3a3a00c16 33 int nTraj = 0; //trajectory index
lukeplummer 2:ea7ef710a07f 34 float aD[2] = {0.0, 0.0};
lukeplummer 2:ea7ef710a07f 35 float traj1[] = {};
lukeplummer 2:ea7ef710a07f 36 float traj2[] = {};
lukeplummer 2:ea7ef710a07f 37 int numPoints = sizeof(traj1)/sizeof(traj1[0]);
lukeplummer 2:ea7ef710a07f 38 float fTraj = numPoints/3.0; //time frequency of trajectory commands
lukeplummer 2:ea7ef710a07f 39 //starting motor positions
lukeplummer 2:ea7ef710a07f 40 float a0_1 = traj1[0];
lukeplummer 2:ea7ef710a07f 41 float a0_2 = traj2[0];
lukeplummer 0:e6c3a3a00c16 42 float e1 = 0;
lukeplummer 0:e6c3a3a00c16 43 float e2 = 0;
lukeplummer 2:ea7ef710a07f 44 //controller gains
lukeplummer 2:ea7ef710a07f 45 float kp = 0.7;
lukeplummer 1:fa246c82ab54 46 float kd = 0.0;
lukeplummer 2:ea7ef710a07f 47
lukeplummer 1:fa246c82ab54 48 bool done = false;
lukeplummer 0:e6c3a3a00c16 49
lukeplummer 1:fa246c82ab54 50 void pdControl() {
lukeplummer 2:ea7ef710a07f 51 float in1 = aD[0];
lukeplummer 2:ea7ef710a07f 52 float in2 = aD[1];
lukeplummer 0:e6c3a3a00c16 53 //get motor position
lukeplummer 0:e6c3a3a00c16 54 a1_t1 = encoder1.getPulses()*2*3.14/1200.0;
lukeplummer 0:e6c3a3a00c16 55 a2_t1 = encoder2.getPulses()*2*3.14/1200.0;
lukeplummer 0:e6c3a3a00c16 56 //calculate error
lukeplummer 0:e6c3a3a00c16 57 e1 = a1_t1-in1;
lukeplummer 0:e6c3a3a00c16 58 e2 = a2_t1-in2;
lukeplummer 0:e6c3a3a00c16 59 //calculate motor speed
lukeplummer 1:fa246c82ab54 60 w1 = abs(a1_t1-a1_t0)*fTraj;
lukeplummer 1:fa246c82ab54 61 w2 = abs(a2_t1-a2_t0)*fTraj;
lukeplummer 0:e6c3a3a00c16 62 //set motor direction
lukeplummer 0:e6c3a3a00c16 63 mDir1_A = (e1<0);
lukeplummer 0:e6c3a3a00c16 64 mDir1_B = !(e1<0);
lukeplummer 0:e6c3a3a00c16 65 mDir2_A = (e2>0);
lukeplummer 0:e6c3a3a00c16 66 mDir2_B = !(e2>0);
lukeplummer 0:e6c3a3a00c16 67 //command motor speed
lukeplummer 1:fa246c82ab54 68 pwmOut1.period(.0001); //set pwm frequency to 10kHz
lukeplummer 1:fa246c82ab54 69 pwmOut2.period(.0001); //set pwm frequency to 10kHz
lukeplummer 0:e6c3a3a00c16 70 pwmOut1.write(abs(kp*e1)+abs(w1*kd));
lukeplummer 0:e6c3a3a00c16 71 pwmOut2.write(abs(kp*e2)+abs(w2*kd));
lukeplummer 0:e6c3a3a00c16 72
lukeplummer 0:e6c3a3a00c16 73 a1_t0 = a1_t1; //save encoder position for next step (to find angular velocity)
lukeplummer 0:e6c3a3a00c16 74 a2_t0 = a2_t1;
lukeplummer 0:e6c3a3a00c16 75 }
lukeplummer 0:e6c3a3a00c16 76
lukeplummer 1:fa246c82ab54 77 void setTraj() {
lukeplummer 2:ea7ef710a07f 78 if (nTraj > numPoints){
lukeplummer 2:ea7ef710a07f 79 //nTraj = 0;
lukeplummer 1:fa246c82ab54 80 done = true;
lukeplummer 1:fa246c82ab54 81 } else {
lukeplummer 1:fa246c82ab54 82 done = false;
lukeplummer 2:ea7ef710a07f 83 aD[0] = traj1[nTraj]-a0_1;
lukeplummer 2:ea7ef710a07f 84 aD[1] = traj2[nTraj]-a0_2;
lukeplummer 2:ea7ef710a07f 85 // aD[1] = traj2[nTraj]-a0_2+aD[0]; //corrects for coaxial motors
lukeplummer 1:fa246c82ab54 86 nTraj++;
lukeplummer 1:fa246c82ab54 87 }
lukeplummer 1:fa246c82ab54 88 }
lukeplummer 1:fa246c82ab54 89
lukeplummer 0:e6c3a3a00c16 90 int main() {
lukeplummer 1:fa246c82ab54 91 mDir1_A = 1;
lukeplummer 1:fa246c82ab54 92 mDir1_B = 0;
lukeplummer 1:fa246c82ab54 93 mDir2_A = 1;
lukeplummer 1:fa246c82ab54 94 mDir2_B = 0;
lukeplummer 1:fa246c82ab54 95
lukeplummer 2:ea7ef710a07f 96 mySerial.printf("numPoints: %d, fTraj: %f, fPWM: %f\n\r", numPoints, fTraj, fPWM);
lukeplummer 2:ea7ef710a07f 97
lukeplummer 2:ea7ef710a07f 98 //get initial position
lukeplummer 2:ea7ef710a07f 99
lukeplummer 2:ea7ef710a07f 100
lukeplummer 1:fa246c82ab54 101 if (sizeof(traj1) == sizeof(traj2)) {
lukeplummer 1:fa246c82ab54 102 trajTicker.attach(setTraj, 1/fTraj);
lukeplummer 1:fa246c82ab54 103 ctrlTicker.attach(pdControl, 1/fPWM);
lukeplummer 2:ea7ef710a07f 104 while (!done) {mySerial.printf("d: %f a: %f \n\r", aD[0], a1_t1);}
lukeplummer 1:fa246c82ab54 105 mySerial.printf("Done\n\r");
lukeplummer 1:fa246c82ab54 106 mDir1_A = 0;
lukeplummer 1:fa246c82ab54 107 mDir1_B = 0;
lukeplummer 1:fa246c82ab54 108 mDir2_A = 0;
lukeplummer 1:fa246c82ab54 109 mDir2_B = 0;
lukeplummer 0:e6c3a3a00c16 110 } else {
lukeplummer 1:fa246c82ab54 111 mySerial.printf("Input error\n\r");
lukeplummer 0:e6c3a3a00c16 112 }
lukeplummer 0:e6c3a3a00c16 113 }