Luke Plummer / Mbed 2 deprecated JesusLizard

lizard leg trajectory pd control code for 2.s994

Dependencies:   QEI mbed

main.cpp@0:e6c3a3a00c16, 2013-11-17 (annotated)

Committer:
lukeplummer
Date:
Sun Nov 17 06:51:53 2013 +0000
Revision:
0:e6c3a3a00c16
Child:
1:fa246c82ab54
initial commit

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 0:e6c3a3a00c16 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 high1 = 1;
lukeplummer 0:e6c3a3a00c16 22 low1 = 0;
lukeplummer 0:e6c3a3a00c16 23 high2 = 1;
lukeplummer 0:e6c3a3a00c16 24 low2 = 0;
lukeplummer 0:e6c3a3a00c16 25
lukeplummer 0:e6c3a3a00c16 26 // Declare other objects
lukeplummer 0:e6c3a3a00c16 27 Ticker trajTicker;
lukeplummer 0:e6c3a3a00c16 28 Ticker ctrlTicker; // creates an instance of the ticker class, which can be used for running functions at a specified frequency.
lukeplummer 0:e6c3a3a00c16 29 Serial mySerial(USBTX, USBRX); // create a serial connection to the computer over the tx/rx pins
lukeplummer 0:e6c3a3a00c16 30
lukeplummer 0:e6c3a3a00c16 31 float a1_t0 = 0; //motor angle 1 from previous time step
lukeplummer 0:e6c3a3a00c16 32 float a1_t1 = 0; //motor angle 1 from current time step
lukeplummer 0:e6c3a3a00c16 33 float a2_t0 = 0; //motor angle 2 from current time step
lukeplummer 0:e6c3a3a00c16 34 float a2_t1 = 0; //motor angle 2 from current time step
lukeplummer 0:e6c3a3a00c16 35 float omega1 = 0;
lukeplummer 0:e6c3a3a00c16 36 float omega2 = 0;
lukeplummer 0:e6c3a3a00c16 37 float fTraj = 1.0; //time frequency of trajectory commands
lukeplummer 0:e6c3a3a00c16 38 int nTraj = 0; //trajectory index
lukeplummer 0:e6c3a3a00c16 39 float Ad1[10] = {0}; //target angles
lukeplummer 0:e6c3a3a00c16 40 float Ad2[10] = {0};
lukeplummer 0:e6c3a3a00c16 41 float e1 = 0;
lukeplummer 0:e6c3a3a00c16 42 float e2 = 0;
lukeplummer 0:e6c3a3a00c16 43 float kp = 1;
lukeplummer 0:e6c3a3a00c16 44 float kd = 1;
lukeplummer 0:e6c3a3a00c16 45
lukeplummer 0:e6c3a3a00c16 46 void getTraj(i) {
lukeplummer 0:e6c3a3a00c16 47
lukeplummer 0:e6c3a3a00c16 48 }
lukeplummer 0:e6c3a3a00c16 49
lukeplummer 0:e6c3a3a00c16 50 void pdControl(float in1, float in2) {
lukeplummer 0:e6c3a3a00c16 51 //get motor position
lukeplummer 0:e6c3a3a00c16 52 a1_t1 = encoder1.getPulses()*2*3.14/1200.0;
lukeplummer 0:e6c3a3a00c16 53 a2_t1 = encoder2.getPulses()*2*3.14/1200.0;
lukeplummer 0:e6c3a3a00c16 54 //calculate error
lukeplummer 0:e6c3a3a00c16 55 e1 = a1_t1-in1;
lukeplummer 0:e6c3a3a00c16 56 e2 = a2_t1-in2;
lukeplummer 0:e6c3a3a00c16 57 //calculate motor speed
lukeplummer 0:e6c3a3a00c16 58 omega1 = abs(a1_t1-a1_t0)*fTraj;
lukeplummer 0:e6c3a3a00c16 59 omega2 = abs(a2_t1-a2_t0)*fTraj;
lukeplummer 0:e6c3a3a00c16 60 //set motor direction
lukeplummer 0:e6c3a3a00c16 61 mDir1_A = (e1<0);
lukeplummer 0:e6c3a3a00c16 62 mDir1_B = !(e1<0);
lukeplummer 0:e6c3a3a00c16 63 mDir2_A = (e2>0);
lukeplummer 0:e6c3a3a00c16 64 mDir2_B = !(e2>0);
lukeplummer 0:e6c3a3a00c16 65 //command motor speed
lukeplummer 0:e6c3a3a00c16 66 pwmOut.period(.0001); //set pwm frequency to 10kHz
lukeplummer 0:e6c3a3a00c16 67 pwmOut1.write(abs(kp*e1)+abs(w1*kd));
lukeplummer 0:e6c3a3a00c16 68 pwmOut2.write(abs(kp*e2)+abs(w2*kd));
lukeplummer 0:e6c3a3a00c16 69 mySerial.printf("Angles: %f, %f \n\r", a2_t1, a1_t1);
lukeplummer 0:e6c3a3a00c16 70
lukeplummer 0:e6c3a3a00c16 71 a1_t0 = a1_t1; //save encoder position for next step (to find angular velocity)
lukeplummer 0:e6c3a3a00c16 72 a2_t0 = a2_t1;
lukeplummer 0:e6c3a3a00c16 73 }
lukeplummer 0:e6c3a3a00c16 74
lukeplummer 0:e6c3a3a00c16 75 int main() {
lukeplummer 0:e6c3a3a00c16 76 if (sizeOf(aD1) == sizeOf(aD2)) {
lukeplummer 0:e6c3a3a00c16 77
lukeplummer 0:e6c3a3a00c16 78 }
lukeplummer 0:e6c3a3a00c16 79 } else {
lukeplummer 0:e6c3a3a00c16 80 mySerial.printf("Input error");
lukeplummer 0:e6c3a3a00c16 81 }
lukeplummer 0:e6c3a3a00c16 82 }