Luke Plummer
/
JesusLizard
lizard leg trajectory pd control code for 2.s994
Revision 0:e6c3a3a00c16, committed 2013-11-17
- Comitter:
- lukeplummer
- Date:
- Sun Nov 17 06:51:53 2013 +0000
- Child:
- 1:fa246c82ab54
- Commit message:
- initial commit
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Sun Nov 17 06:51:53 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Nov 17 06:51:53 2013 +0000 @@ -0,0 +1,82 @@ +// THE FIGHTING BANANA SLUGS!!! + +#include "mbed.h" // mbed library +#include "QEI.h" // quadrature encoder library to count encoder ticks + +//Setup +//Motor 1 +DigitalOut mDir1_A(p5); +DigitalOut mDir1_B(p6) +AnalogIn aIn1(p19); +PwmOut pwmOut1(p21); +QEI encoder1(p23, p24, NC, 1200, QEI::X4_ENCODING); + +//Motor 2 +DigitalOut mDir2_A(p11); +DigitalOut mDir2_B(p12); +AnalogIn aIn2(p20); +PwmOut pwmOut2(p22); +QEI encoder2(p25, p26, NC, 1200, QEI::X4_ENCODING); + +high1 = 1; +low1 = 0; +high2 = 1; +low2 = 0; + +// Declare other objects +Ticker trajTicker; +Ticker ctrlTicker; // creates an instance of the ticker class, which can be used for running functions at a specified frequency. +Serial mySerial(USBTX, USBRX); // create a serial connection to the computer over the tx/rx pins + +float a1_t0 = 0; //motor angle 1 from previous time step +float a1_t1 = 0; //motor angle 1 from current time step +float a2_t0 = 0; //motor angle 2 from current time step +float a2_t1 = 0; //motor angle 2 from current time step +float omega1 = 0; +float omega2 = 0; +float fTraj = 1.0; //time frequency of trajectory commands +int nTraj = 0; //trajectory index +float Ad1[10] = {0}; //target angles +float Ad2[10] = {0}; +float e1 = 0; +float e2 = 0; +float kp = 1; +float kd = 1; + +void getTraj(i) { + +} + +void pdControl(float in1, float in2) { + //get motor position + a1_t1 = encoder1.getPulses()*2*3.14/1200.0; + a2_t1 = encoder2.getPulses()*2*3.14/1200.0; + //calculate error + e1 = a1_t1-in1; + e2 = a2_t1-in2; + //calculate motor speed + omega1 = abs(a1_t1-a1_t0)*fTraj; + omega2 = abs(a2_t1-a2_t0)*fTraj; + //set motor direction + mDir1_A = (e1<0); + mDir1_B = !(e1<0); + mDir2_A = (e2>0); + mDir2_B = !(e2>0); + //command motor speed + pwmOut.period(.0001); //set pwm frequency to 10kHz + pwmOut1.write(abs(kp*e1)+abs(w1*kd)); + pwmOut2.write(abs(kp*e2)+abs(w2*kd)); + mySerial.printf("Angles: %f, %f \n\r", a2_t1, a1_t1); + + a1_t0 = a1_t1; //save encoder position for next step (to find angular velocity) + a2_t0 = a2_t1; +} + +int main() { + if (sizeOf(aD1) == sizeOf(aD2)) { + + } + } else { + mySerial.printf("Input error"); + } +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sun Nov 17 06:51:53 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/a9913a65894f \ No newline at end of file