Diff: main.cpp

Revision:

5:ca4f81348c30

Parent:

4:89f7d7f3a7ca

Child:

6:3dcd9ce47900

--- a/main.cpp	Tue Oct 25 15:23:25 2016 +0000
+++ b/main.cpp	Tue Oct 25 17:38:55 2016 +0000
@@ -3,6 +3,7 @@
 #include "stdio.h"
 #include "MODSERIAL.h"
 #include "QEI.h"
+#include "BiQuad.h"
 // Angle limits 215, 345 lower arm, 0, 145 upper arm
 // Robot arm x,y limits (limit to table top)
 
@@ -47,15 +48,15 @@
 DigitalOut motor1dir(D7);
 PwmOut motor1pwm(D6);
 QEI motor1sense(D13,D12,NC, 8400);
-float motor1pos;
-float motor1int;
-float motor1olderr;
+double motor1pos=0;
+double motor1int=0;
+double motor1olderr=0;
 DigitalOut motor2dir(D4);
 PwmOut motor2pwm(D5);
-QEI motor2sense(D11,D10, NC, 8400);
-float motor2pos;
-float motor2int;
-float motor2olderr;
+QEI motor2sense(D10,D11,NC, 8400);
+double motor2pos=0;
+double motor2int=0;
+double motor2olderr=0;
 
 
 float vx=0;
@@ -72,8 +73,8 @@
 MODSERIAL pc(USBTX, USBRX);
 DigitalIn switch1(SW3);
 DigitalIn switch2(SW2);
-DigitalIn switch3(D7);
-DigitalIn switch4(D9);
+//DigitalIn switch3(D7);
+//DigitalIn switch4(D9);
 DigitalOut ledr(LED_RED);
 DigitalOut ledg(LED_GREEN);
 
@@ -251,6 +252,9 @@
     end.x = 0; 
     end.y = arm1.length+arm2.length;
     end.theta = 0;
+    
+    motor1sense.reset();
+    motor2sense.reset();
 }
 
 int lr_state=0;
@@ -263,7 +267,7 @@
 
 void inputswitches()
 {
-        if(switch1.read())
+/*        if(switch1.read())
         {
             if (lr_state==0){
                 vx=0;
@@ -329,6 +333,7 @@
             }
             ledr.write(0);
         }
+        */
         /*
         if (switch1.read()){
             end.x += spd;
@@ -358,6 +363,9 @@
         else
         {
             lr_state=0;
+            motor1int = 0;
+            motor2int = 0;
+
             vx = vx - ax;
             if (vx<-3.0f){
                 vx=-3.0f;
@@ -369,35 +377,38 @@
         {
             if(lr_state==1){
                 vx=0;
+
                 ledr.write(1);
             }
         }
         else
         {
             lr_state=1;
+            motor1int = 0;
+            motor2int = 0;
             vx = vx + ax;
             if (vx>3.0f){
                 vx=3.0f;
             }
             ledr.write(0);
         }
-        robot_setSetpoint(end.x+vx, end.y+vy);
+        robot_setSetpoint(end.x+vx, end.y);
     }
 }
 
 void setmotor1(float val){
     motor1dir.write(val>=0?1:0);
-    motor1pwm.write(fabs(val)>1?1.0f:fabs(val));
+    motor1pwm.write(fabs(val)>1?1.0f:sqrt(fabs(val)));
 }
 void setmotor2(float val){
     motor2dir.write(val>=0?1:0);
-    motor2pwm.write(fabs(val)>1?1.0f:fabs(val));
+    motor2pwm.write(fabs(val)>1?1.0f:sqrt(fabs(val)));
 }
 
 
-double PID_control(float& olderr, float position, float setpoint, float& integrator, float dt, float P, float I, float D){
-    float error;
-    float derivative;
+double PID_control(double& olderr, double position, double setpoint, double& integrator, double dt, double P, double I, double D){
+    double error;
+    double derivative;
     error = setpoint - position;
     integrator += error * dt;
     derivative = (error - olderr) / dt;
@@ -406,16 +417,21 @@
 }
 
 void r_doPID(){
-    float dt = 1/50.0;
-    float m1, m2;
+    double dt = 1/50.0;
+    double m1;
+    double m2;
+    
     if (flag_PID){
         flag_PID = false;
-        motor1pos = (float)motor1sense.getPulses()/8400;
-        motor2pos = (float)motor2sense.getPulses()/8400;
-        m1 = PID_control(motor1olderr, motor1pos, arm1.theta, motor1int, dt, 1.0, 0,0);
-        m2 = PID_control(motor2olderr, motor2pos, arm2.theta, motor2int, dt, 1.0, 0,0);
+        motor1pos = (double)motor1sense.getPulses()*(2*M_PI/8400.0);
+        motor2pos = (double)motor2sense.getPulses()*(2*M_PI/8400.0);
+        m1 = PID_control(motor1olderr, motor1pos, arm1.theta, motor1int, dt, 1.0, 0.2, 0.2);
+        m2 = PID_control(motor2olderr, motor2pos, arm2.theta, motor2int, dt, 1.0, 0.2, 0.2);
         setmotor1(m1);
         setmotor2(m2);
+        printf("e1: %f, e2: %f\n\r", motor1olderr, motor2olderr);
+        printf("p1: %f, p2: %f\n\r", motor1pos, motor2pos);
+        printf("m1: %f, m2: %f\n\r", m1, m2);
     }
 }
 
@@ -423,9 +439,14 @@
     if (flag_output){
         flag_output = false;    
         printf("Angle1\n\r");
-        printf("%f\n\r", motor1pos);
+        printf("%f\n\r", arm1.theta);
         printf("Angle2\n\r");
+        printf("%f\n\r", arm2.theta);
+        printf("Pos1\n\r");
+        printf("%f\n\r", motor1pos);
+        printf("Pos2\n\r");
         printf("%f\n\r", motor2pos);
+        
         printf("x: %f | y: %f\n\r", end.x, end.y);
     }
 }
@@ -441,6 +462,7 @@
     if (cnt%100 == 0){  // 10 times per second
         flag_output = true;
     }
+    cnt++;
 }
 
 int main(){