Diff: main.cpp

Revision:

0:e6c3a3a00c16

Child:

1:fa246c82ab54

--- /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