Luke Plummer / Mbed 2 deprecated JesusLizard

lizard leg trajectory pd control code for 2.s994

Dependencies:   QEI mbed

main.cpp@10:6ecef8d1a018, 2013-12-01 (annotated)

Committer:
lukeplummer
Date:
Sun Dec 01 22:25:40 2013 +0000
Revision:
10:6ecef8d1a018
Parent:
9:80c1f8bbcd55 
added button code, removed file write code, fixed trigger code

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 10:6ecef8d1a018 19 DigitalIn button(p7);
lukeplummer 6:8cd6aa47fd51 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 10:6ecef8d1a018 24 Ticker buttonTicker;
lukeplummer 0:e6c3a3a00c16 25 Serial mySerial(USBTX, USBRX); // create a serial connection to the computer over the tx/rx pins
lukeplummer 0:e6c3a3a00c16 26
lukeplummer 9:80c1f8bbcd55 27 DigitalOut dOut1(p17);
lukeplummer 9:80c1f8bbcd55 28
lukeplummer 0:e6c3a3a00c16 29 float a1_t0 = 0; //motor angle 1 from previous time step
lukeplummer 0:e6c3a3a00c16 30 float a1_t1 = 0; //motor angle 1 from current time step
lukeplummer 0:e6c3a3a00c16 31 float a2_t0 = 0; //motor angle 2 from current time step
lukeplummer 0:e6c3a3a00c16 32 float a2_t1 = 0; //motor angle 2 from current time step
lukeplummer 1:fa246c82ab54 33 float w1 = 0;
lukeplummer 1:fa246c82ab54 34 float w2 = 0;
lukeplummer 2:ea7ef710a07f 35 float fPWM = 1000;
lukeplummer 0:e6c3a3a00c16 36 int nTraj = 0; //trajectory index
lukeplummer 2:ea7ef710a07f 37 float aD[2] = {0.0, 0.0};
lukeplummer 3:6f00e4876cab 38 //knee
lukeplummer 6:8cd6aa47fd51 39 float traj2[] = {0.0};
lukeplummer 3:6f00e4876cab 40 //hip
lukeplummer 6:8cd6aa47fd51 41 float traj1[] = {0.0};
lukeplummer 2:ea7ef710a07f 42 int numPoints = sizeof(traj1)/sizeof(traj1[0]);
lukeplummer 3:6f00e4876cab 43 float fTraj = numPoints/0.5; //time frequency of trajectory commands
lukeplummer 2:ea7ef710a07f 44 //starting motor positions
lukeplummer 3:6f00e4876cab 45 float a1_0 = traj1[0];
lukeplummer 3:6f00e4876cab 46 float a2_0 = traj2[0];
lukeplummer 0:e6c3a3a00c16 47 float e1 = 0;
lukeplummer 0:e6c3a3a00c16 48 float e2 = 0;
lukeplummer 2:ea7ef710a07f 49 //controller gains
lukeplummer 3:6f00e4876cab 50 float kp1 = 1.7;
lukeplummer 3:6f00e4876cab 51 float kd1 = 0.0;
lukeplummer 3:6f00e4876cab 52 float kp2 = 1.5;
lukeplummer 3:6f00e4876cab 53 float kd2 = 0.0;
lukeplummer 2:ea7ef710a07f 54
lukeplummer 1:fa246c82ab54 55 bool done = false;
lukeplummer 0:e6c3a3a00c16 56
lukeplummer 1:fa246c82ab54 57 void pdControl() {
lukeplummer 2:ea7ef710a07f 58 float in1 = aD[0];
lukeplummer 2:ea7ef710a07f 59 float in2 = aD[1];
lukeplummer 0:e6c3a3a00c16 60 //get motor position
lukeplummer 0:e6c3a3a00c16 61 a1_t1 = encoder1.getPulses()*2*3.14/1200.0;
lukeplummer 0:e6c3a3a00c16 62 a2_t1 = encoder2.getPulses()*2*3.14/1200.0;
lukeplummer 0:e6c3a3a00c16 63 //calculate error
lukeplummer 0:e6c3a3a00c16 64 e1 = a1_t1-in1;
lukeplummer 0:e6c3a3a00c16 65 e2 = a2_t1-in2;
lukeplummer 0:e6c3a3a00c16 66 //calculate motor speed
lukeplummer 1:fa246c82ab54 67 w1 = abs(a1_t1-a1_t0)*fTraj;
lukeplummer 1:fa246c82ab54 68 w2 = abs(a2_t1-a2_t0)*fTraj;
lukeplummer 0:e6c3a3a00c16 69 //set motor direction
lukeplummer 0:e6c3a3a00c16 70 mDir1_A = (e1<0);
lukeplummer 0:e6c3a3a00c16 71 mDir1_B = !(e1<0);
lukeplummer 0:e6c3a3a00c16 72 mDir2_A = (e2>0);
lukeplummer 0:e6c3a3a00c16 73 mDir2_B = !(e2>0);
lukeplummer 0:e6c3a3a00c16 74 //command motor speed
lukeplummer 1:fa246c82ab54 75 pwmOut1.period(.0001); //set pwm frequency to 10kHz
lukeplummer 1:fa246c82ab54 76 pwmOut2.period(.0001); //set pwm frequency to 10kHz
lukeplummer 3:6f00e4876cab 77 pwmOut1.write(abs(kp1*e1)+abs(w1*kd1));
lukeplummer 3:6f00e4876cab 78 pwmOut2.write(abs(kp2*e2)+abs(w2*kd2));
lukeplummer 0:e6c3a3a00c16 79
lukeplummer 0:e6c3a3a00c16 80 a1_t0 = a1_t1; //save encoder position for next step (to find angular velocity)
lukeplummer 0:e6c3a3a00c16 81 a2_t0 = a2_t1;
lukeplummer 0:e6c3a3a00c16 82 }
lukeplummer 0:e6c3a3a00c16 83
lukeplummer 1:fa246c82ab54 84 void setTraj() {
lukeplummer 3:6f00e4876cab 85 if (nTraj >= numPoints){
lukeplummer 3:6f00e4876cab 86 nTraj = 0;
lukeplummer 3:6f00e4876cab 87 //done = true;
lukeplummer 1:fa246c82ab54 88 } else {
lukeplummer 1:fa246c82ab54 89 done = false;
lukeplummer 3:6f00e4876cab 90 aD[0] = traj1[nTraj]-a1_0;
lukeplummer 3:6f00e4876cab 91 aD[1] = traj2[nTraj]-a2_0;
lukeplummer 1:fa246c82ab54 92 nTraj++;
lukeplummer 1:fa246c82ab54 93 }
lukeplummer 1:fa246c82ab54 94 }
lukeplummer 1:fa246c82ab54 95
lukeplummer 10:6ecef8d1a018 96 void checkBtn() {
lukeplummer 10:6ecef8d1a018 97 if (button.read() == 1){
lukeplummer 10:6ecef8d1a018 98 done = true;
lukeplummer 10:6ecef8d1a018 99 }
lukeplummer 6:8cd6aa47fd51 100 }
lukeplummer 6:8cd6aa47fd51 101
lukeplummer 0:e6c3a3a00c16 102 int main() {
lukeplummer 9:80c1f8bbcd55 103 wait(5.0);
lukeplummer 1:fa246c82ab54 104 mDir1_A = 1;
lukeplummer 1:fa246c82ab54 105 mDir1_B = 0;
lukeplummer 1:fa246c82ab54 106 mDir2_A = 1;
lukeplummer 1:fa246c82ab54 107 mDir2_B = 0;
lukeplummer 1:fa246c82ab54 108
lukeplummer 10:6ecef8d1a018 109 //mySerial.printf("numPoints: %d, fTraj: %f, fPWM: %f\n\r", numPoints, fTraj, fPWM);
lukeplummer 2:ea7ef710a07f 110
lukeplummer 2:ea7ef710a07f 111 //get initial position
lukeplummer 9:80c1f8bbcd55 112
lukeplummer 9:80c1f8bbcd55 113 dOut1.write(1);
lukeplummer 2:ea7ef710a07f 114
lukeplummer 1:fa246c82ab54 115 if (sizeof(traj1) == sizeof(traj2)) {
lukeplummer 1:fa246c82ab54 116 trajTicker.attach(setTraj, 1/fTraj);
lukeplummer 1:fa246c82ab54 117 ctrlTicker.attach(pdControl, 1/fPWM);
lukeplummer 10:6ecef8d1a018 118 buttonTicker.attach(checkBtn, 0.1);
lukeplummer 4:af5440b3af19 119 while (!done) {
lukeplummer 10:6ecef8d1a018 120 //mySerial.printf("motor 2: %f \n\r", a2_t1);
lukeplummer 4:af5440b3af19 121 }
lukeplummer 10:6ecef8d1a018 122 //mySerial.printf("Done\n\r");
lukeplummer 10:6ecef8d1a018 123 dOut1.write(0);
lukeplummer 1:fa246c82ab54 124 mDir1_A = 0;
lukeplummer 1:fa246c82ab54 125 mDir1_B = 0;
lukeplummer 1:fa246c82ab54 126 mDir2_A = 0;
lukeplummer 1:fa246c82ab54 127 mDir2_B = 0;
lukeplummer 0:e6c3a3a00c16 128 } else {
lukeplummer 10:6ecef8d1a018 129 //mySerial.printf("Input error\n\r");
lukeplummer 0:e6c3a3a00c16 130 }
lukeplummer 0:e6c3a3a00c16 131 }