Diff: main.cpp

Revision:

1:fe4a0b47ff25

Parent:

0:1b64a0cedc5d

Child:

2:4e082e4c255d

--- a/main.cpp	Mon Mar 03 15:27:32 2014 +0000
+++ b/main.cpp	Mon Mar 03 21:37:24 2014 +0000
@@ -14,9 +14,10 @@
 #define OIL_RIG1        1
 #define OIL_RIG2        2
 
-void restingState(void);
+void servoBegin(void);
 void initServoDriver(void);
 void setServoPulse(uint8_t n, float pulse);
+void setServoPulseNo_delay(uint8_t n, float pulse);
 void servoPosition(int set);
 
 /************
@@ -46,16 +47,15 @@
 *******************/
 //lrf_baudCalibration();
 initServoDriver();
-restingState();
-servoPosition(STORE_POSITION);
-ServoOutputDisable = 0;
+servoBegin();
+//servoPosition(STORE_POSITION);
+//ServoOutputDisable = 0;
 while(1){
         if(pc.readable()){
-            pc.scanf("%d %d %d", &outputDisabled, &servoNum, &pulseWidth);
-            //pc.scanf("%d", &posNum);
-            //servoPosition(posNum);
+            //pc.scanf("%d %d %d", &outputDisabled, &servoNum, &pulseWidth);
+             pc.scanf("%d %d", &servoNum, &pulseWidth);
              setServoPulse(servoNum, pulseWidth);
-             ServoOutputDisable = outputDisabled;
+            
         }
 }
 
@@ -91,16 +91,27 @@
 **************/
 
 void setServoPulse(uint8_t n, float pulse) {
-   /* float pulselength = 20000;   // 20,000 us per second
-    if (currentPosition[n] < pulse){
-        for (int i=currentPosition[n]; i<(pulse+1); i++)
-        pulse = 4094 * currentPosition[n]+1 / pulselength;
-        pwm.setPWM(n, 0, pulse);
-        } else {
-            }*/
-       float pulselength = 20000;   // 20,000 us per second     
-       pulse = 4094 * currentPosition[n]+1 / pulselength;
-       pwm.setPWM(n, 0, pulse);        
+    float pulselength = 20000;   // 20,000 us per second
+    int i = currentPosition[n];
+    pc.printf("\ncurrent position = %d\n", currentPosition[n]);
+    int pulse2; 
+    if(currentPosition[n] < pulse){
+          pc.printf("\ncurrent position < pulse\n");
+          for(i; i < pulse; i++){  
+          pulse2 = 4094 * i / pulselength;
+          pwm.setPWM(n, 0, pulse2);
+          wait_ms(3); 
+          }
+    } else if (currentPosition[n] > pulse) {
+           pc.printf("\ncurrent position > pulse\n");
+           for(i; i > pulse; i--){  
+           pulse2 = 4094 * i / pulselength;
+           pwm.setPWM(n, 0, pulse2);
+           wait_ms(3);
+           }
+    }
+    currentPosition[n] = i;
+    pc.printf("\nending position = %d\n\n", i);  
 }
 
 void initServoDriver(void) {
@@ -108,29 +119,29 @@
     //pwm.setPWMFreq(100);  //This dosen't work well because of uncertain clock speed. Use setPrescale().
     pwm.setPrescale(140);    //This value is decided for 20ms interval.
     pwm.setI2Cfreq(400000); //400kHz
+    
 }
 
-void restingState(void){
-    /*
-    currentPosition[0] = 2300;
-    currentPosition[1] = 500;
-    currentPosition[2] = 600;
-    currentPosition[3] = 2450;
-    currentPosition[4] = 2450;
-    currentPosition[5] = 0;
-    currentPosition[6] = 0;
-    */
+void servoBegin(void){
+    setServoPulseNo_delay(0, 900);
+    setServoPulseNo_delay(1, 500);
+    setServoPulseNo_delay(2, 600);
+    setServoPulseNo_delay(3, 2450);
+    setServoPulseNo_delay(4, 2450); 
+    setServoPulseNo_delay(5, 0);
+    setServoPulseNo_delay(6, 0); 
+    }
     
-    setServoPulse(0, 2300);
-    setServoPulse(1, 500);
-    setServoPulse(2, 600);
-    setServoPulse(3, 2450);
-    setServoPulse(4, 2450); 
-    }
+void setServoPulseNo_delay(uint8_t n, float pulse) {
+    float pulselength = 20000;   // 20,000 us per second
+    currentPosition[n] = pulse;
+    pulse = 4094 * pulse / pulselength;
+    pwm.setPWM(n, 0, pulse);
+}
     
 void servoPosition(int set){   
             //moves to current position
-            setServoPulse(1, Arm_Table[set].base_arm,);
+            setServoPulse(1, Arm_Table[set].base_arm);
             setServoPulse(0, Arm_Table[set].base_rotate);
             setServoPulse(2, Arm_Table[set].lil_arm);
             setServoPulse(3, Arm_Table[set].big_arm);