Diff: main.cpp

Revision:

2:2ef793d0ac01

Parent:

1:4eeae0f92c4c

--- a/main.cpp	Sat Apr 20 07:02:28 2019 +0000
+++ b/main.cpp	Sat Apr 20 09:35:37 2019 +0000
@@ -14,6 +14,9 @@
 DigitalOut csEnc2(PB_12);           //SPI2_SSEL Encoder 1
 DigitalOut csEnc1(PA_4);            //SPI3_SSEL Encoder 2
 
+Ticker encTicker;
+
+
 DigitalOut dir1(PC_0);
 PwmOut pwm1(PC_9);
 
@@ -29,6 +32,8 @@
 char encArr[2]= {255,255};
 char encLength = 2;
 bool start = false;
+float q1 = 0;
+float q2 = 0;
 
 
 /////////////////////////////
@@ -78,6 +83,12 @@
     return ConvertAngle(encData/2);
 }
 
+//ISR reads encoders every 20us
+void readEncoders(){
+    q1 = readEnc1();
+    q2 = readEnc2();
+    }
+
 //drive motor joint1
 void drvMotor1(float duty){
     if(duty > 0.9){
@@ -98,7 +109,7 @@
     pwm1.write(duty);
 }
 
-//drive motor joint1
+//drive motor joint2
 void drvMotor2(float duty){
     if(duty > 0.5){
         duty = 0.5;
@@ -107,10 +118,10 @@
         duty = -0.5;
     }
     if(duty >= 0){
-        dir2 = 1; //CCW
+        dir2 = 0; //CCW
     }
     else if(duty < 0){
-        dir2 = 0; //CW
+        dir2 = 1; //CW
         duty = -duty;
     }
     pwm2.period_ms(1.0f);
@@ -126,14 +137,12 @@
     bool stop1 = false;
     
     while(stop1==false){
-        float encData1 = readEnc1();
-        float encData2 = readEnc2();
-        printf("Encoder1: %.2f  ", encData1);
-        printf("Encoder2: %.2f\n", encData2);
-        if(encData1 < 110){
-            encData1 = encData1 + 360;
+        printf("Encoder1: %.2f  ", q1);
+        printf("Encoder2: %.2f\n", q2);
+        if(q1 < 110){
+            q1 = q1 + 360;
         }
-        e = (pos - encData1) + offset;
+        e = (pos - q1) + offset;
         s = s + e;
         if(abs(e)>0.5){
             u = (Kp*e) + (Ki*s) + (Kd*(e-p));
@@ -156,11 +165,9 @@
     bool stop = false;
     pos=-pos;
     while(stop==false){
-        float encData1 = readEnc1();
-        float encData2 = readEnc2();
-        printf("Encoder1: %.2f  ", encData1);
-        printf("Encoder2: %.2f\n", encData2);
-        e = (pos - encData2) + offset;
+        printf("Encoder1: %.2f  ", q1);
+        printf("Encoder2: %.2f\n", q2);
+        e = (pos - q2) + offset;
         s = s + e;
         if(abs(e)>0.5){
             u = (Kp*e) + (Ki*s) + (Kd*(e-p));
@@ -183,6 +190,8 @@
 //Main
 
 int main() {
+    encTicker.attach_us(&readEncoders, 2.0);
+    
     pc.baud(256000);
     RX_INT();
     pc.attach(&RX_INT, Serial::RxIrq);
@@ -216,24 +225,12 @@
             if(array[2] == 121){ //'y'
                 setHome();
             }
+            if(array[2] == 120){ //'x'
+                printf('q1 = %.2f, q2 = %.2)
             
             getData = 0;
         }
-        //test driving joint 1 and joint 2
-              PID_2(45);
-        wait(0.5);
-        PID_1(90);
-        wait(0.1);
-        PID_2(30);
-        wait(3);
-        PID_1(0);
-        wait(0.1);
-        PID_2(-45);
-        wait(100);
         
     }
     
-    
-    
-    
 }
\ No newline at end of file