Diff: main.cpp

Revision:

3:ff08522e6416

Parent:

2:a94e3ade9632

--- a/main.cpp	Mon Feb 24 22:53:26 2014 +0000
+++ b/main.cpp	Mon Mar 03 13:36:17 2014 +0000
@@ -1,44 +1,51 @@
 #include "mbed.h"
 #include "Adafruit_PWMServoDriver.h"
 
+#define SERVOMIN  150 // this is the 'minimum' pulse length count (out of 4096)
+#define SERVOMAX  600 // this is the 'maximum' pulse length count (out of 4096)
+
 Adafruit_PWMServoDriver pwm(p28, p27);
 Serial pc(USBTX, USBRX);
-int num, input;
+int input;
+char num;
 
 
-typedef struct { int Arm1; int Arm2; int Arm3; int Arm4; int Arm5; } Coord;
-
-Coord Arm_Table[] = 
+int Arm_Table[][7] = 
 {
-   //Arm1   Arm2   Arm3   Arm4   Arm5
-   //Claw                        Base 
-      
-    {0, 1700, 1650, 1650, 0},// straight up 
-    {0, 2700, 600, 600, 0}//storing position
+   //Arm0   Arm1   Arm2   Arm3   Arm4   Arm5    Arm 6
+   //Base                                       Claw 
+     
+    {2500, 550, 600, 2700, 2700, 0, 0},// storing  
+    {1300, 800, 1150, 2500, 2700, 0, 0},// straight up 
+    {2350, 1050, 700, 500, 2350, 0, 0}// over tools
 
 }; 
 
 
 
 void setServoPulse(uint8_t n, float pulse) {
-    static int prev[6];
+    static int prev[7]={2500, 550, 600, 2700, 2700, 0, 0};
     int inc, loop, i, value, set;
     float pulselength = 10000;   // 10,000 us per second
     
-    if(pulse>=500 && pulse<=2700){
+   if(pulse>=500 && pulse<=2700){
         inc=abs(pulse-prev[n]);
         
-        if(inc <500){
-            inc=inc/16;
-            loop=16;
+        if(inc <55){
+            inc=inc/10;
+            loop=10;
         }
-        else if(inc< 1000){
+        else if(inc <500){
             inc=inc/32;
             loop=32;
         }
+        else if(inc< 1000){
+            inc=inc/70;
+            loop=70;
+        }
         else{
-            inc= inc/64;
-            loop=64;
+            inc= inc/100;
+            loop=100;
         }
 
         value= prev[n] + inc;
@@ -60,12 +67,10 @@
 }
 
 void setPosition( int set ){
-
-    setServoPulse(1, Arm_Table[set].Arm1);  
-    setServoPulse(2, Arm_Table[set].Arm2);
-    setServoPulse(3, Arm_Table[set].Arm3);
-    setServoPulse(4, Arm_Table[set].Arm4);
-    setServoPulse(5, Arm_Table[set].Arm5);  
+    int i;
+    
+    for(i=0; i<7; i++)setServoPulse(i, Arm_Table[set][i]);  
+ 
 
 }
 
@@ -76,17 +81,82 @@
     pwm.setI2Cfreq(400000); //400kHz
 }
 
+
+
+
 int main() {
+
+    int move[7]={2500, 550, 600, 2700, 2700, 0, 0};
+    int i, delta=50;
     //pwm.i2c_probe();
     initServoDriver();
-    setPosition(1);
+    setPosition(0);
+    
+    setPosition(1);// laser range find
     setPosition(0);
-    setPosition(1); 
+    
+    setPosition(2); //tool find 
+    setPosition(0);
+     
     
     while(1){
         if(pc.readable()){
-             pc.scanf("%d %d", &num, &input);
-             setServoPulse(num, input);
+             num=pc.getc();
+             
+             if(num == 'a' || num == 'z'){
+                 if(move[0]>2650)move[0]=2700;
+                 else if(move[0] < 550) move[0]=500;
+                 
+                 if(num=='a'){
+                     setServoPulse(0, (move[0]+=delta));
+                 }
+                 else setServoPulse(0, (move[0]-=delta));
+             }
+             else if(num == 's' || num == 'x'){
+                 if(move[1]> 2650)move[1]=2700;
+                 else if(move[1] < 550) move[1]=500;
+                 
+                 if(num=='s')setServoPulse(1, (move[1]+=delta));
+                 else setServoPulse(1, (move[1]-=delta));
+             }
+             else if(num == 'd' || num == 'c'){
+                 if(move[2]> 2650)move[2]=2700;
+                 else if(move[2] < 550) move[2]=500;
+                 
+                 if(num=='d')setServoPulse(2, (move[2]+=delta));
+                 else setServoPulse(2, (move[2]-=delta));
+             }
+             else if(num == 'f' || num == 'v'){
+                 if(move[3]> 2650)move[3]=2700;
+                 else if(move[3] < 550) move[3]=500;
+                 
+                 if(num=='f')setServoPulse(3, (move[3]+=delta));
+                 else setServoPulse(3, (move[3]-=delta));
+             }
+             else if(num == 'g' || num == 'b'){
+                 if(move[4]> 2650)move[4]=2700;
+                 else if(move[4] < 550) move[4]=500;
+                 
+                 if(num=='g')setServoPulse(4, (move[4]+=delta));
+                 else setServoPulse(4, (move[4]-=delta));
+             }
+             else if(num == 'h' || num == 'n'){
+                 if(move[5]> 2650)move[5]=2700;
+                 else if(move[5] < 550) move[5]=500;
+                 
+                 if(num=='h')setServoPulse(5, (move[5]+=delta));
+                 else setServoPulse(5, (move[5]-=delta));
+             }
+             else if(num == 'j' || num == 'n'){
+                 if(move[6]> 2650)move[6]=2700;
+                 else if(move[6] < 550) move[6]=500;
+                 
+                 if(num=='j')setServoPulse(6, (move[6]+=delta));
+                 else setServoPulse(6, (move[6]-=delta));
+             }
+             pc.printf("0\t 1\t 2\t 3 \t4 \t 5 \t 6\n\r");
+             pc.printf("%d\t %d\t %d\t %d \t%d \t %d \t %d\n\r",move[0],move[1], move[2], move[3],move[4],move[5],move[6]);
+             //setServoPulse(num, input);
         }
     }