Diff: main.cpp

Revision:

5:c74f23e4325f

Parent:

4:0c1555b01361

Child:

6:414eb29191ad

--- a/main.cpp	Fri Jul 08 08:54:39 2016 +0000
+++ b/main.cpp	Sun Jul 10 04:23:11 2016 +0000
@@ -18,8 +18,8 @@
 
 DigitalOut myled(LED1);
 uint8_t pos = 1;
-uint16_t tiltInc = 100; // was 15 for RX
-uint16_t panInc = 100;
+uint16_t tiltInc = 50; // was 15 for RX
+uint16_t panInc = 50;
 int echo;
 
 void centrePosition() {
@@ -42,7 +42,7 @@
     
     Base.setGoalPosition(150);
     Elbow.setGoalPosition(150);
-    Wrist.setGoalPosition(200);
+    Wrist.setGoalPosition(180);
     pos = 2;
     wait(2);
 }
@@ -56,18 +56,22 @@
         centrePosition();
         currentPosBase = Base.getPresentPosition();
         currentPosElbow = Elbow.getPresentPosition();
-        while(currentPosBase < 145 && currentPosElbow > 155) {
+        while(currentPosBase < 140 && currentPosElbow > 160) {
             currentPosBase = Base.getPresentPosition();
             currentPosElbow = Elbow.getPresentPosition();
         }
     }
-    Wrist.setMovingSpeed(1024+80);             // SPEEEEEEEED
+    Wrist.setMovingSpeed(1024+80); 
+    if  (pos == 4) {
+        Wrist.setMovingSpeed(1024+50);        
+    }
+                // SPEEEEEEEED
     Elbow.setMovingSpeed(1024+50);          //Speeed
     Base.setMovingSpeed(1024+30);          //Speeed
     
     Base.setGoalPosition(70);
     Elbow.setGoalPosition(60);
-    Wrist.setGoalPosition(400);
+    Wrist.setGoalPosition(370);
     pos = 3;
     wait(2);
 }
@@ -88,14 +92,18 @@
     }
     if  (pos == 3) {
         Base.setGoalPosition(150);      // To lessen lever disadvantage
-        wait(0.5);
+        Elbow.setGoalPosition(150);
+        Wrist.setMovingSpeed(1024+50);
+        Wrist.setGoalPosition(140);
+        wait(1);
     }
     
-    Wrist.setMovingSpeed(1024+70);             // SPEEEEEEEED
+    Wrist.setMovingSpeed(1024+110);             // SPEEEEEEEED
     Elbow.setMovingSpeed(1024+50);          //Speeed
     Base.setMovingSpeed(1024+100);          //Speeed
     Base.setGoalPosition(350); 
     Elbow.setGoalPosition(50);
+    wait_us(100);
     Wrist.setGoalPosition(60);
     pos = 4;
     wait(2);
@@ -105,7 +113,7 @@
     uint16_t currentPosBase;
     uint16_t currentPosElbow;
     Wrist.setMovingSpeed(1024+100);             // SPEEEEEEEED
-    
+    Base.setMovingSpeed(1024+100);
     if (pos == 3) {
         centrePosition();
         currentPosBase = Base.getPresentPosition();
@@ -130,8 +138,8 @@
     //}
     
     Base.setGoalPosition(60);
-    Elbow.setGoalPosition(360);
-    Wrist.setGoalPosition(65);
+    Elbow.setGoalPosition(350);
+    Wrist.setGoalPosition(60);
     pos = 1;
     wait(2);
 }
@@ -156,6 +164,8 @@
     Base.setMovingSpeed(1024+speed);
     Elbow.setMovingSpeed(1024+2.3*speed); // was 1024+2.2*speed
     Wrist.setMovingSpeed(1024+1.4*speed);  
+    Pan.setMovingSpeed(1024+0.3*speed);
+    Tilt.setMovingSpeed(1024+0.3*speed);
 }
 
 void setLimits() {
@@ -172,7 +182,7 @@
     
     currentPosTilt = Tilt.getPresentPosition();
     Tilt.setGoalPosition(currentPosTilt-tiltInc);
-    wait_ms(500);
+    wait_ms(10);
 }
 
 void tiltIncrementDown() {
@@ -180,15 +190,15 @@
     
     currentPosTilt = Tilt.getPresentPosition();
     Tilt.setGoalPosition(currentPosTilt+tiltInc);
-    wait_ms(500);
+    wait_ms(10);
 }
 
 void panIncrementLeft() {
     uint16_t currentPosPan;
     
     currentPosPan = Pan.getPresentPosition();
-    Pan.setGoalPosition(currentPosPan-panInc);
-    wait_ms(500);
+    Pan.setGoalPosition(currentPosPan+panInc);
+    wait_ms(10);
     
     /*
     currentPosPan = Pan.getPresentPosition();
@@ -201,8 +211,8 @@
     uint16_t currentPosPan;
     
     currentPosPan = Pan.getPresentPosition();
-    Pan.setGoalPosition(currentPosPan+panInc);
-    wait_ms(500);
+    Pan.setGoalPosition(currentPosPan-panInc);
+    wait_ms(10);
     
     /*
     currentPosPan = Pan.getPresentPosition();
@@ -219,12 +229,12 @@
     uint16_t currentPosPan;
     Pan.setCWAngleLimit(0);
     Pan.setCCWAngleLimit(4095);
-    Pan.setGoalPosition(300);
-    Tilt.setGoalPosition(1250);
+    Pan.setGoalPosition(1850);
+    Tilt.setGoalPosition(195);
     
     Base.setGoalPosition(60);
-    Elbow.setGoalPosition(360);
-    Wrist.setGoalPosition(65);
+    Elbow.setGoalPosition(350);
+    Wrist.setGoalPosition(50);
     while(1) {
         //if (echo == 0) {  
             //Base.setGoalPosition(1300);
@@ -234,7 +244,7 @@
          
         echo = pc.getc();
     
-        pc.printf("\r\n %d \r\n", echo);
+        //pc.printf("\r\n %d \r\n", echo);
         pc.putc(echo);
         if (echo == 49) {    retractedPosition();    }
         else if (echo == 50) {    centrePosition();  }
@@ -246,17 +256,16 @@
         else if (echo == 97) {    panIncrementLeft();   }
         else if (echo == 100) {   panIncrementRight();   }
         else {  
-            Tilt.setMovingSpeed(1024);
-            Pan.setMovingSpeed(1024);
+            //Tilt.setMovingSpeed(1024);
+            //Pan.setMovingSpeed(1024);
             currentPosPan = Pan.getPresentPosition();
             Pan.setGoalPosition(currentPosPan);
         }
-        
     
         currentPosTilt = Tilt.getPresentPosition();
-        //pc.printf("Tilt Position: %d\r\n",currentPosTilt);
+        pc.printf("Tilt Position: %d\r\n",currentPosTilt);
         currentPosPan = Pan.getPresentPosition();
-        //pc.printf("Pan Position: %d\r\n",currentPosPan);
+        pc.printf("Pan Position: %d\r\n",currentPosPan);
         
             
         /*