Diff: main.cpp

Revision:

2:a94e3ade9632

Parent:

0:4323102e4255

Child:

3:ff08522e6416

--- a/main.cpp	Sat Aug 17 09:06:58 2013 +0000
+++ b/main.cpp	Mon Feb 24 22:53:26 2014 +0000
@@ -1,12 +1,72 @@
 #include "mbed.h"
 #include "Adafruit_PWMServoDriver.h"
 
-Adafruit_PWMServoDriver pwm(p9, p10);
+Adafruit_PWMServoDriver pwm(p28, p27);
+Serial pc(USBTX, USBRX);
+int num, input;
+
+
+typedef struct { int Arm1; int Arm2; int Arm3; int Arm4; int Arm5; } Coord;
+
+Coord Arm_Table[] = 
+{
+   //Arm1   Arm2   Arm3   Arm4   Arm5
+   //Claw                        Base 
+      
+    {0, 1700, 1650, 1650, 0},// straight up 
+    {0, 2700, 600, 600, 0}//storing position
+
+}; 
+
+
 
 void setServoPulse(uint8_t n, float pulse) {
+    static int prev[6];
+    int inc, loop, i, value, set;
     float pulselength = 10000;   // 10,000 us per second
-    pulse = 4094 * pulse / pulselength;
-    pwm.setPWM(n, 0, pulse);
+    
+    if(pulse>=500 && pulse<=2700){
+        inc=abs(pulse-prev[n]);
+        
+        if(inc <500){
+            inc=inc/16;
+            loop=16;
+        }
+        else if(inc< 1000){
+            inc=inc/32;
+            loop=32;
+        }
+        else{
+            inc= inc/64;
+            loop=64;
+        }
+
+        value= prev[n] + inc;
+        
+        for(i=0; i<loop; i++){
+            set = 4094 * value / pulselength;
+            pwm.setPWM(n, 0, set);
+            if( prev[n] < pulse)value+= inc;
+            else value-= inc;
+            wait_ms(15);
+        }
+
+        
+        prev[n]= pulse; 
+    
+        pulse = 4094 * pulse / pulselength;
+        pwm.setPWM(n, 0, pulse);
+    }
+}
+
+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);  
+
 }
 
 void initServoDriver() {
@@ -19,7 +79,16 @@
 int main() {
     //pwm.i2c_probe();
     initServoDriver();
-    setServoPulse(0, 1000);
+    setPosition(1);
+    setPosition(0);
+    setPosition(1); 
     
-    while(1);
+    while(1){
+        if(pc.readable()){
+             pc.scanf("%d %d", &num, &input);
+             setServoPulse(num, input);
+        }
+    }
+    
+
 }