Diff: main.cpp

Revision:

41:08d41bb622bb

Parent:

40:c40bd1a84e7c

Child:

42:3aa03b5cefb0

--- a/main.cpp	Fri Nov 03 03:00:30 2017 +0000
+++ b/main.cpp	Fri Nov 03 03:36:49 2017 +0000
@@ -23,7 +23,7 @@
     Ticker      max_read1;
     Ticker      max_read3;
     Ticker      Motorcontrol;
-    //Ticker      PIDtimer;
+    Ticker      tencoder;
     HIDScope    scope( 4 );
     DigitalOut  red(LED_RED);
     DigitalOut  blue(LED_BLUE);
@@ -517,6 +517,9 @@
             //MV1 = 0.5;
             //MV2 = 0;
             x = x + 0.1;
+            if (x > 16) {
+            x = 15;
+            }
             double alpha = inversekinematics1(x,y); //calculate alpha and beta for current x,y
             double beta = inversekinematics2(x,y);
             setpoint1 = alpha;
@@ -532,6 +535,9 @@
             //MV1 = -0.5;
             //MV2 = 0;
             x = x - 0.1;
+            if (x < 10.77){
+            x = 10.77;
+            }
             double alpha = inversekinematics1(x,y);
             double beta = inversekinematics2(x,y);
             setpoint1 = alpha;
@@ -546,6 +552,9 @@
             //MV1 = 0;
             //MV2 = 0.5;
             y = y + 0.1;
+            if (y > 19.5) {
+            y = 19.5;
+            }
             double alpha = inversekinematics1(x,y);
             double beta = inversekinematics2(x,y);
             setpoint1 = alpha;
@@ -560,6 +569,9 @@
             //MV1 = 0;
             //MV2 = -0.5;
             y = y - 0.1;
+            if (y < 15.73) {
+            y = 15.73;
+            }
             double alpha = inversekinematics1(x,y);
             double beta = inversekinematics2(x,y);
             setpoint1 = alpha;
@@ -615,7 +627,14 @@
     scope.send(); // send everything to the HID scope 
     
 }
+
+void encoders(){
+        count1 = Encoder1.getPulses();
+        count2 = Encoder2.getPulses();
+}
+
 /*
+
 // PID calculations
 //
 //
@@ -668,25 +687,26 @@
     //PIDcalculation1();
     //filter();
     while (angle1<setpoint1 || angle1>setpoint1 || angle2<setpoint2 || angle2>setpoint2) {
+        encoders();
+        double count1;
+        double count2;
+        angle1 += (0.0981 * count1);
+        angle2 += (0.0981 * count2);
         if (angle1<setpoint1 && angle2<setpoint2) {
         motor1direction = 1; // counterclockwise rotation
         motor2direction = 1;
-        
         }
         else if (angle1>setpoint1 && angle2<setpoint2) {
         motor1direction = 0; // clockwise rotation
         motor2direction = 1;
-        
         }
         else if (angle1<setpoint1 && angle2>setpoint2) {
         motor1direction = 1;
         motor2direction = 0;
-        
         }
         else if (angle1>setpoint1 && angle2>setpoint2) {
         motor1direction = 0;
         motor2direction = 0;
-        
         }
         if ((angle1<setpoint1 || angle1>setpoint1) && (angle2<setpoint2 || angle2>setpoint2)) {
         motor1pwm = 0.2;
@@ -704,10 +724,6 @@
         motor1pwm = 0;
         motor2pwm = 0;
         }
-        double count1;
-        double count2;
-        angle1 += (0.0981 * count1);
-        angle2 += (0.0981 * count2);
     }
 }
 /*
@@ -732,22 +748,21 @@
         }
     }
 }
-*/
+
 void MeasureAndControl(void) {
     SetMotor1();
     //SetMotor2();
 }
-
+*/
     
 int main(){   
     pc.baud(115200);
     main_timer.attach(&filter, 0.001); // set frequency for the filters at 1000Hz
     max_read1.attach(&get_max1, 2); // set the frequency of the calibration loop at 0.5Hz
     max_read3.attach(&get_max3, 2);
-    Motorcontrol.attach(&MeasureAndControl,0.1);
+    tencoder.attach(&encoders, 0.001);
+    Motorcontrol.attach(&SetMotor1,0.1);
     //PIDtimer.attach(&PIDcalculation1, 0.005);
     //PIDtimer.attach(&PIDcalculation2, 0.005);
-    count1 = Encoder1.getPulses();
-    count2 = Encoder2.getPulses();
     while(1) {}
 }
\ No newline at end of file