Diff: main.cpp

Revision:

3:587441455259

Parent:

2:4e082e4c255d

Child:

4:116829a5ae3c

--- a/main.cpp	Mon Mar 03 22:30:34 2014 +0000
+++ b/main.cpp	Thu Mar 06 00:12:56 2014 +0000
@@ -30,6 +30,7 @@
 void setServoPulse(uint8_t n, float pulse);
 void setServoPulseNo_delay(uint8_t n, float pulse);
 void servoPosition(int set);
+int ServoTest(void);
 
 /************
 Variables for Servos
@@ -48,7 +49,7 @@
       
     {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, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
+    {STORE_POSITION, 2350, 1050, 700, 500, 2350, 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
@@ -58,6 +59,7 @@
 
 int main() {
     
+      
 /*****************
  INITIALIZATIONS
 *******************/
@@ -71,16 +73,26 @@
 ServoOutputDisable = 0;
 
 while(1){
-        if(pc.readable()){
-            /*
-             pc.scanf("%d %d %d", &outputDisabled, &servoNum, &pulseWidth);
-             setServoPulse(servoNum, pulseWidth);
-            */
-            pc.scanf("%d", &posNum);
-            servoPosition(posNum);
+    int instr;
+        
+    printf("Set Servo Position = 0, Shape Detect = 1, Oil Rig Detect = 2");
+    while(!pc.readable());
+    pc.scanf("%d", &instr);
+    
+    if(instr == 0){// Servo Control
+        while(!ServoTest());
+    }
+    else if( instr == 1){// shape detect 
+        //servoPosition(3);
+        
+        
+    }
+    else if( instr == 2){// oil rig detect
+            servoPosition(2);
             
-            
-        }
+    }
+
+}// End Main while        
 }
 
 /**************************************************
@@ -103,7 +115,7 @@
     
     //OILRIG 3 DISTANCE READING
 
-}
+
 
 
 
@@ -115,21 +127,33 @@
 
 void setServoPulse(uint8_t n, float pulse) {
     float pulselength = 20000;   // 20,000 us per second
-    int i = currentPosition[n];
+    int i = currentPosition[n], pstart;
     pc.printf("\ncurrent position = %d\n", currentPosition[n]);
-    int pulse2; 
+    int pulse2, pulse1; 
+    
+    // Set when to start the pulse for the different servos
+    // and normalize values 
+    if(n < 2){
+        pstart = 0;// need to change this to match the different servos
+        pulse1= pulse + 500;
+    }    
+    else{
+        pstart = 0;
+        pulse1=pulse+500;     
+    }    
+    
     if(currentPosition[n] < pulse){
           pc.printf("\ncurrent position < pulse\n");
-          for(i; i < pulse; i++){  
+          for(i; i < pulse1; i++){  
           pulse2 = 4094 * i / pulselength;
-          pwm.setPWM(n, 0, pulse2);
+          pwm.setPWM(n, pstart, pulse2);
           wait_ms(3); 
           }
     } else if (currentPosition[n] > pulse) {
            pc.printf("\ncurrent position > pulse\n");
-           for(i; i > pulse; i--){  
+           for(i; i > pulse1; i--){  
            pulse2 = 4094 * i / pulselength;
-           pwm.setPWM(n, 0, pulse2);
+           pwm.setPWM(n, pstart, pulse2);
            wait_ms(3);
            }
     }
@@ -173,5 +197,67 @@
             setServoPulse(6, Arm_Table[set].claw_open); 
 }
 
+int ServoTest(void){
+    int num, delta=50;  // Change Delta to  change increment size
+    static int move[7]={900, 500, 600, 2450, 2450, 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(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));
+        }  
+        else if(num== 'e') return 1;
+                
+        pc.printf("0\t 1\t 2\t 3 \t4 \t 5 \t 6\t type e to end\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]);
+    }
+    return 0;
+}
 
-