Luke Plummer
/
JesusLizard
lizard leg trajectory pd control code for 2.s994
main.cpp@1:fa246c82ab54, 2013-11-18 (annotated)
- Committer:
- lukeplummer
- Date:
- Mon Nov 18 20:41:14 2013 +0000
- Revision:
- 1:fa246c82ab54
- Parent:
- 0:e6c3a3a00c16
- Child:
- 2:ea7ef710a07f
it works! only tested with one motor, some strange behaivor
Who changed what in which revision?
User | Revision | Line number | New 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 | 1:fa246c82ab54 | 32 | float fTraj = 0.5; //time frequency of trajectory commands |
lukeplummer | 1:fa246c82ab54 | 33 | float fPWM = 100; |
lukeplummer | 0:e6c3a3a00c16 | 34 | int nTraj = 0; //trajectory index |
lukeplummer | 1:fa246c82ab54 | 35 | float traj1[] = {0.0, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0}; |
lukeplummer | 1:fa246c82ab54 | 36 | float traj2[] = {0.0, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0}; |
lukeplummer | 1:fa246c82ab54 | 37 | float Ad[2] = {0.0, 0.0}; |
lukeplummer | 1:fa246c82ab54 | 38 | int numPoints; |
lukeplummer | 1:fa246c82ab54 | 39 | //float traj1[] = {0.7854,0.7854,0.78538,0.78488,0.7817,0.77462,0.76353,0.74871,0.73061,0.70985,0.68715,0.66323,0.63881,0.61442,0.59051,0.56748,0.54558,0.52503,0.50594,0.48841,0.4725,0.45823,0.44563,0.43472,0.42551,0.41801,0.41224,0.40823,0.40603,0.40571,0.4073,0.41054,0.41521,0.42106,0.42787,0.43545,0.44363,0.45228,0.46129,0.47054,0.47995,0.48943,0.49893,0.50835,0.51762,0.52666,0.53572,0.54496,0.55436,0.56391,0.57363,0.58349,0.59352,0.60389,0.61482,0.62627,0.63823,0.6507,0.66367,0.67717,0.69121,0.7058,0.72099,0.73683,0.75335,0.77064,0.7888,0.80797,0.82834,0.85026,0.87437,0.9016,0.92915,0.95698,0.98204,1.0049,1.0263,1.0466,1.0662,1.0849,1.103,1.1205,1.1374,1.1536,1.1693,1.1844,1.1989,1.2128,1.2261,1.2389,1.251,1.2626,1.2735,1.2838,1.2935,1.3027,1.3111,1.319,1.3263,1.3329,1.339,1.3444,1.3492,1.3535,1.3572,1.3603,1.3629,1.365,1.3666,1.3678,1.3685,1.3688,1.3686,1.3679,1.3667,1.3651,1.363,1.3603,1.3571,1.3532,1.3485,1.343,1.3367,1.3295,1.3213,1.312,1.3016,1.29,1.2771,1.2628,1.2471,1.2297,1.2112,1.1922,1.173,1.1541,1.1347,1.1145,1.0933,1.0696,1.0427,1.0124,0.97855,0.9409,0.89924,0.85172}; |
lukeplummer | 1:fa246c82ab54 | 40 | //float traj2[] = {0.0,0.0,0.0,0.00063466,0.0045936,0.013526,0.027801,0.047381,0.071993,0.10117,0.13426,0.17054,0.20922,0.24967,0.29134,0.3337,0.37633,0.41893,0.46125,0.50311,0.5444,0.58501,0.62488,0.66395,0.70219,0.73957,0.77608,0.81171,0.84653,0.88097,0.91256,0.94131,0.9672,0.99033,1.0108,1.0289,1.0446,1.0583,1.07,1.08,1.0883,1.0952,1.1007,1.105,1.1081,1.1103,1.1116,1.1121,1.1119,1.1109,1.1092,1.1068,1.1036,1.0996,1.0946,1.0887,1.0818,1.0738,1.0646,1.0544,1.0429,1.0302,1.0163,1.001,0.98433,0.96625,0.94662,0.92532,0.90212,0.87666,0.84819,0.8156,0.78232,0.74848,0.71788,0.68994,0.66369,0.63865,0.61459,0.59136,0.56886,0.54703,0.52581,0.50515,0.48502,0.46539,0.44622,0.4275,0.40921,0.39132,0.37382,0.35668,0.33991,0.32348,0.30739,0.29163,0.27618,0.26106,0.24624,0.23174,0.21755,0.20368,0.19014,0.17694,0.16409,0.15161,0.13952,0.12787,0.11633,0.10487,0.093503,0.082227,0.071049,0.059976,0.049015,0.038174,0.027463,0.016894,0.0062166,-0.0046431,-0.01562,-0.026652,-0.037675,-0.04862,-0.059416,-0.069981,-0.080228,-0.090055,-0.099352,-0.10799,-0.11583,-0.1227,-0.12829,-0.13237,-0.13496,-0.13617,-0.13614,-0.13485,-0.1322,-0.12782,-0.12111,-0.11153,-0.098492,-0.081307,-0.059248,-0.030488}; |
lukeplummer | 0:e6c3a3a00c16 | 41 | float e1 = 0; |
lukeplummer | 0:e6c3a3a00c16 | 42 | float e2 = 0; |
lukeplummer | 1:fa246c82ab54 | 43 | float kp = 0.5; |
lukeplummer | 1:fa246c82ab54 | 44 | float kd = 0.0; |
lukeplummer | 1:fa246c82ab54 | 45 | bool done = false; |
lukeplummer | 0:e6c3a3a00c16 | 46 | |
lukeplummer | 1:fa246c82ab54 | 47 | void pdControl() { |
lukeplummer | 1:fa246c82ab54 | 48 | float in1 = Ad[0]; |
lukeplummer | 1:fa246c82ab54 | 49 | float in2 = Ad[1]; |
lukeplummer | 0:e6c3a3a00c16 | 50 | //get motor position |
lukeplummer | 0:e6c3a3a00c16 | 51 | a1_t1 = encoder1.getPulses()*2*3.14/1200.0; |
lukeplummer | 0:e6c3a3a00c16 | 52 | a2_t1 = encoder2.getPulses()*2*3.14/1200.0; |
lukeplummer | 0:e6c3a3a00c16 | 53 | //calculate error |
lukeplummer | 0:e6c3a3a00c16 | 54 | e1 = a1_t1-in1; |
lukeplummer | 0:e6c3a3a00c16 | 55 | e2 = a2_t1-in2; |
lukeplummer | 0:e6c3a3a00c16 | 56 | //calculate motor speed |
lukeplummer | 1:fa246c82ab54 | 57 | w1 = abs(a1_t1-a1_t0)*fTraj; |
lukeplummer | 1:fa246c82ab54 | 58 | w2 = abs(a2_t1-a2_t0)*fTraj; |
lukeplummer | 0:e6c3a3a00c16 | 59 | //set motor direction |
lukeplummer | 0:e6c3a3a00c16 | 60 | mDir1_A = (e1<0); |
lukeplummer | 0:e6c3a3a00c16 | 61 | mDir1_B = !(e1<0); |
lukeplummer | 0:e6c3a3a00c16 | 62 | mDir2_A = (e2>0); |
lukeplummer | 0:e6c3a3a00c16 | 63 | mDir2_B = !(e2>0); |
lukeplummer | 0:e6c3a3a00c16 | 64 | //command motor speed |
lukeplummer | 1:fa246c82ab54 | 65 | pwmOut1.period(.0001); //set pwm frequency to 10kHz |
lukeplummer | 1:fa246c82ab54 | 66 | pwmOut2.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 | |
lukeplummer | 0:e6c3a3a00c16 | 70 | a1_t0 = a1_t1; //save encoder position for next step (to find angular velocity) |
lukeplummer | 0:e6c3a3a00c16 | 71 | a2_t0 = a2_t1; |
lukeplummer | 0:e6c3a3a00c16 | 72 | } |
lukeplummer | 0:e6c3a3a00c16 | 73 | |
lukeplummer | 1:fa246c82ab54 | 74 | void setTraj() { |
lukeplummer | 1:fa246c82ab54 | 75 | if (nTraj >= numPoints){ |
lukeplummer | 1:fa246c82ab54 | 76 | done = true; |
lukeplummer | 1:fa246c82ab54 | 77 | } else { |
lukeplummer | 1:fa246c82ab54 | 78 | done = false; |
lukeplummer | 1:fa246c82ab54 | 79 | Ad[0] = traj1[nTraj]; |
lukeplummer | 1:fa246c82ab54 | 80 | Ad[1] = traj2[nTraj]; |
lukeplummer | 1:fa246c82ab54 | 81 | nTraj++; |
lukeplummer | 1:fa246c82ab54 | 82 | } |
lukeplummer | 1:fa246c82ab54 | 83 | } |
lukeplummer | 1:fa246c82ab54 | 84 | |
lukeplummer | 0:e6c3a3a00c16 | 85 | int main() { |
lukeplummer | 1:fa246c82ab54 | 86 | mDir1_A = 1; |
lukeplummer | 1:fa246c82ab54 | 87 | mDir1_B = 0; |
lukeplummer | 1:fa246c82ab54 | 88 | mDir2_A = 1; |
lukeplummer | 1:fa246c82ab54 | 89 | mDir2_B = 0; |
lukeplummer | 1:fa246c82ab54 | 90 | |
lukeplummer | 1:fa246c82ab54 | 91 | if (sizeof(traj1) == sizeof(traj2)) { |
lukeplummer | 1:fa246c82ab54 | 92 | numPoints = sizeof(traj1)/4; |
lukeplummer | 1:fa246c82ab54 | 93 | trajTicker.attach(setTraj, 1/fTraj); |
lukeplummer | 1:fa246c82ab54 | 94 | ctrlTicker.attach(pdControl, 1/fPWM); |
lukeplummer | 1:fa246c82ab54 | 95 | while (!done) {mySerial.printf("d: %f a: %f \n\r", Ad[0], a1_t1);} |
lukeplummer | 1:fa246c82ab54 | 96 | mySerial.printf("Done\n\r"); |
lukeplummer | 1:fa246c82ab54 | 97 | mDir1_A = 0; |
lukeplummer | 1:fa246c82ab54 | 98 | mDir1_B = 0; |
lukeplummer | 1:fa246c82ab54 | 99 | mDir2_A = 0; |
lukeplummer | 1:fa246c82ab54 | 100 | mDir2_B = 0; |
lukeplummer | 0:e6c3a3a00c16 | 101 | } else { |
lukeplummer | 1:fa246c82ab54 | 102 | mySerial.printf("Input error\n\r"); |
lukeplummer | 0:e6c3a3a00c16 | 103 | } |
lukeplummer | 0:e6c3a3a00c16 | 104 | } |