Diff: main.cpp

Revision:

10:45473f650198

Parent:

9:e4c34f5665a0

Child:

11:773b3532d50f

--- a/main.cpp	Wed Oct 19 15:38:44 2016 +0000
+++ b/main.cpp	Wed Oct 19 16:14:25 2016 +0000
@@ -94,8 +94,8 @@
     float biceps2 = button2.read();
     if (biceps1 > 0 && biceps2 > 0){
         //both arms activated: stamp moves down
-        //led1 = 1;
-        //led2 = 1;
+        led1 = 1;
+        led2 = 1;
         dx = 0;
         dy = dy_stampdown; //into stamping vertical position?? ~the stamp down action
          wait(1);
@@ -103,21 +103,21 @@
         }
     else if (biceps1 > 0 && biceps2 <= 0){
         //arm 1 activated, move left
-        //led1 = 1;
-        //led2 = 0;
+        led1 = 1;
+        led2 = 0;
         dx = -biceps1;
         dy = 0;
         }
     else if (biceps1 <= 0 && biceps2 > 0){
         //arm 1 activated, move left
-        //led1 = 0;
-        //led2 = 1;
+        led1 = 0;
+        led2 = 1;
         dx = biceps2;
         dy = 0;
         }
     else{
-        //led1 = 0;
-        //led2 = 0;
+        led1 = 0;
+        led2 = 0;
         dx=0;
         dy=0;
         }
@@ -188,12 +188,12 @@
     //control motor 2
     if (motorValue2 >=0) 
         {motor2DirectionPin=cw;
-        led1=1;
-        led2=0;
+        //led1=1;
+        //led2=0;
         }
     else {motor2DirectionPin=ccw;
-        led1=0;
-        led2=1;
+        //led1=0;
+        //led2=1;
         }
     if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
         else motor2MagnitudePin = fabs(motorValue2);
@@ -230,19 +230,21 @@
 int main()
 {
  //Initialize
+ int led1val = led1.read();
+ int led2val = led2.read();
+  
+  /*
   pc.baud(115200);
- pc.printf("Test putty");
- led1=1;
-    led1 = 0;
-    led2 = 0;
-    wait(2.0);
-    led1 = 1;
-    led2 = 1;
-    wait(2.0);
+ pc.printf("Test putty ledvals IK");
+    while (true) {
+        wait(0.2f);
+        pc.printf("Led1: %i \r\n", led1val);
+        pc.printf("Led2: %i \r\n", led2val);    
+    }
+    */
  MeasureTicker.attach(&MeasureTicker_act, 1.0f); 
- //bqc.add(&bq1).add(&bq2);
- //QEI Encoder1(D12, D13, NC, 32); // turns on encoder
- /*
+ bqc.add(&bq1).add(&bq2);
+ 
  while(1)
     {
         if (MeasureTicker_go){
@@ -250,14 +252,13 @@
             MeasureAndControl();
             counts1 = Encoder1.getPulses();           // gives position of encoder 
             counts2 = Encoder2.getPulses();           // gives position of encoder 
-            pc.printf("Resolution: %f pulses/rad \r\n",resolution);
+            //pc.printf("Resolution: %f pulses/rad \r\n",resolution);
             }
-
+/*
         if (BiQuadTicker_go){
             BiQuadTicker_go=false;
             BiQuadFilter();
         }
-    
+  */  
     }
-*/
 }
\ No newline at end of file