Diff: main.cpp

Revision:

4:116829a5ae3c

Parent:

3:587441455259

--- a/main.cpp	Thu Mar 06 00:12:56 2014 +0000
+++ b/main.cpp	Sat Mar 08 23:23:35 2014 +0000
@@ -3,7 +3,7 @@
 #include "ShapeDetect.h"
 
 Serial pc(USBTX,USBRX);
-Adafruit_PWMServoDriver pwm(p9,p10);
+Adafruit_PWMServoDriver pwm(p28,p27);
 DigitalOut ServoOutputDisable(p8);
 extern Serial lrf;
 
@@ -47,9 +47,9 @@
    // POSITION ODER:
    // base_rotate, base_arm, lil_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open
       
-    {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
-    {OIL_RIG1, 1500, 1400, 1900, 900, 900, 0, 0}, // point laser at oilrig2
-    {STORE_POSITION, 2350, 1050, 700, 500, 2350, 0, 0}, // Shape Detect
+    {STORE_POSITION, 46, 0, 0, 100, 96, 0, 0}, // storing position
+    {OIL_RIG1, 83, 75, 100, 33, 33, 0, 0}, // point laser at oilrig2
+    {STORE_POSITION, 100, 17, 17, 0, 100, 0, 0}, // Shape Detect
     {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
     {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
     {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
@@ -75,7 +75,7 @@
 while(1){
     int instr;
         
-    printf("Set Servo Position = 0, Shape Detect = 1, Oil Rig Detect = 2");
+    pc.printf("Set Servo Position = 0, Shape Detect = 1, Oil Rig Detect = 2");
     while(!pc.readable());
     pc.scanf("%d", &instr);
     
@@ -128,37 +128,38 @@
 void setServoPulse(uint8_t n, float pulse) {
     float pulselength = 20000;   // 20,000 us per second
     int i = currentPosition[n], pstart;
-    pc.printf("\ncurrent position = %d\n", currentPosition[n]);
-    int pulse2, pulse1; 
+    int pulse2, min=500, max=2700; 
     
     // Set when to start the pulse for the different servos
     // and normalize values 
-    if(n < 2){
+ /*   if(n < 2){
         pstart = 0;// need to change this to match the different servos
-        pulse1= pulse + 500;
+        pulse1= pulse ;
     }    
     else{
         pstart = 0;
-        pulse1=pulse+500;     
-    }    
+        pulse1=pulse;     
+    } */   
+    
+    pulse= ((pulse*(max-min))/100)+min;
     
     if(currentPosition[n] < pulse){
-          pc.printf("\ncurrent position < pulse\n");
-          for(i; i < pulse1; i++){  
+          pc.printf("\ncurrent position < pulse\n\r");
+          for(i=currentPosition[n]; i < pulse; i++){  
           pulse2 = 4094 * i / pulselength;
           pwm.setPWM(n, pstart, pulse2);
-          wait_ms(3); 
+          wait_ms(1); 
           }
     } else if (currentPosition[n] > pulse) {
-           pc.printf("\ncurrent position > pulse\n");
-           for(i; i > pulse1; i--){  
+           pc.printf("\ncurrent position > pulse\n\r");
+           for(i=currentPosition[n]; i > pulse; i--){  
            pulse2 = 4094 * i / pulselength;
            pwm.setPWM(n, pstart, pulse2);
-           wait_ms(3);
+           wait_ms(1);
            }
     }
     currentPosition[n] = i;
-    pc.printf("\nending position = %d\n\n", i);  
+    pc.printf("\nending position = %d\n\n\r", i);  
 }
 
 void initServoDriver(void) {
@@ -170,17 +171,20 @@
 }
 
 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(0, 46);
+    setServoPulseNo_delay(1, 0);
+    setServoPulseNo_delay(2, 0);
+    setServoPulseNo_delay(3, 100);
+    setServoPulseNo_delay(4, 96); 
     setServoPulseNo_delay(5, 0);
     setServoPulseNo_delay(6, 0); 
     }
     
 void setServoPulseNo_delay(uint8_t n, float pulse) {
     float pulselength = 20000;   // 20,000 us per second
+    int min=500, max=2700; 
+    
+    pulse= ((pulse*(max-min))/100)+min;
     currentPosition[n] = pulse;
     pulse = 4094 * pulse / pulselength;
     pwm.setPWM(n, 0, pulse);
@@ -198,57 +202,57 @@
 }
 
 int ServoTest(void){
-    int num, delta=50;  // Change Delta to  change increment size
-    static int move[7]={900, 500, 600, 2450, 2450, 0, 0};
+    int num, delta=1;  // Change Delta to  change increment size
+    static int move[7]={46, 0, 0, 100, 96, 0, 0};
     
     if(pc.readable()){
         num=pc.getc();
              
         if(num == 'a' || num == 'z'){
-                if(move[0]>2650)move[0]=2700;
-                else if(move[0] < 550) move[0]=500;
+                if(move[0]>=(100-delta))move[0]=(100-delta);
+                else if(move[0] <= (0+delta)) move[0]=delta;
                  
                 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(move[1]>=(100-delta))move[1]=(100-delta);
+                else if(move[1] <= (0+delta)) move[1]=delta;
                  
                 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(move[2]>=(100-delta))move[2]=(100-delta);
+                else if(move[2] <= (0+delta)) move[2]=delta;
                  
                 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(move[3]>=(100-delta))move[3]=(100-delta);
+                else if(move[3] <= (0+delta)) move[3]=delta;
                  
                 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(move[4]>=(100-delta))move[4]=(100-delta);
+                else if(move[4] <= (0+delta)) move[4]=delta;
                  
                 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(move[5]>=(100-delta))move[5]=(100-delta);
+                else if(move[5] <= (0+delta)) move[5]=delta;
                  
                 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(move[6]>=(100-delta))move[6]=(100-delta);
+                else if(move[6] <= (0+delta)) move[6]=delta;
                  
                 if(num=='j')setServoPulse(6, (move[6]+=delta));
                 else setServoPulse(6, (move[6]-=delta));