Diff: main.cpp

Revision:

3:b7b4780a7f6e

Parent:

2:4e082e4c255d

Child:

4:42460f387701

--- a/main.cpp	Mon Mar 03 22:30:34 2014 +0000
+++ b/main.cpp	Thu Mar 13 17:06:34 2014 +0000
@@ -3,12 +3,10 @@
 #include "ShapeDetect.h"
 
 Serial pc(USBTX,USBRX);
-Adafruit_PWMServoDriver pwm(p9,p10);
+Adafruit_PWMServoDriver pwm(p28,p27);
 DigitalOut ServoOutputDisable(p8);
 extern Serial lrf;
 
-
-
 //Servo Positions
 #define STORE_POSITION  0
 #define OIL_RIG1        1
@@ -24,83 +22,132 @@
 #define INSERT_TOOL_2   11
 #define INSERT_TOOL_3   11
 
+#define MIN_SERVO_PULSE     900
+#define MAX_SERVO_PULSE     2100
+#define SERVO_MAX_ANGLE     180
+
 
 void servoBegin(void);
 void initServoDriver(void);
-void setServoPulse(uint8_t n, float pulse);
-void setServoPulseNo_delay(uint8_t n, float pulse);
+void setServoPulse(uint8_t n, int angle);
+void setServoPulseNo_delay(uint8_t n, int angle);
 void servoPosition(int set);
+void setGripper(int open);
 
 /************
 Variables for Servos
 *************/
-int servoNum, pulseWidth, outputDisabled, posNum;
-int currentPosition[7]; 
+int servoNum, servoAngle, outputDisabled, posNum, testVal;
+int currentPosition[7];
 
-typedef struct {int arm_action; int base_rotate; int base_arm; 
-                int lil_arm; int big_arm; int claw_arm; 
-                int claw_rotate; int claw_open;} Coord;
- 
-Coord Arm_Table[] = 
-{
-   // 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
+typedef struct {
+    int arm_action;
+    int base_rotate;
+    int base_arm;
+    int lil_arm;
+    int big_arm;
+    int claw_arm;
+    int claw_rotate;
+    int claw_open;
+} Coord;
+
+Coord Arm_Table[] = {
+    // POSITION ODER:
+    // base_rotate, base_arm, lil_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open
+
+    {STORE_POSITION, 900, 900, 900, 900, 900, 900, 900}, // 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, 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
-    {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
+
+};
+
+
+/* Variables for imageprocessing and distance */
+int x_coord;
+int y_coord;
+int area;
+int shape;
+
 
-}; 
+
+
+
 
-int main() {
-    
-/*****************
- INITIALIZATIONS
-*******************/
-//pc.baud(115200); 
+int main()
+{
+
+    /*****************
+     INITIALIZATIONS
+    *******************/
+    pc.baud(115200);
 //Laser Range Finder Initialization
-//lrf_baudCalibration();
- 
+    lrf_baudCalibration();
+
 //Servo initialization
-initServoDriver();
-servoBegin(); // initiates servos to start position
-ServoOutputDisable = 0;
+    initServoDriver();
+    //  servoBegin(); // initiates servos to start position
+//ServoOutputDisable = 0;
+
+    while(1) {
+        pc.printf("PICK A TEST TO PERFORM\n");
+        pc.printf("1) Distance Reading\n");
+        pc.printf("2) Shape Detection\n");
+        pc.printf("3) Servo Control\n");
+        pc.printf("4) Motor Control\n");
+
+        pc.scanf("%d", &testVal);
 
-while(1){
-        if(pc.readable()){
-            /*
-             pc.scanf("%d %d %d", &outputDisabled, &servoNum, &pulseWidth);
-             setServoPulse(servoNum, pulseWidth);
-            */
-            pc.scanf("%d", &posNum);
-            servoPosition(posNum);
-            
-            
+        switch (testVal) {
+            case 1:
+                pc.printf("Distance = %d\n", getDistance());
+                break;
+            case 2:
+                shape =  shapeDetection_mass();
+                printImageToFile(BINARY);
+                break;
+            case 3:
+                servoBegin();
+                pc.scanf("%d %d", &servoNum, &servoAngle);
+                setServoPulse(servoNum, servoAngle);
+                break;
+            default:
+                pc.printf("ERROR: NOT A VALID TEST PROCEDURE");
+                break;
         }
-}
+
+
+        pc.printf("Distance = %d", getDistance());
+
+        /*if(pc.readable()) {
+
+            pc.scanf("%d %d", &servoNum, &pulseWidth);
+            setServoPulse(servoNum, pulseWidth);
+
+
+           // pc.scanf("%d", &posNum);
+            //servoPosition(posNum);
+
 
-/**************************************************
-*     FIRST STAGE 
-*               
-*          - DETERMINE OIL RIG ON FIRE
-*          - DETERMINE PATH
-*   
-**************************************************/
-    
+        }*/
+    }
+
+    /**************************************************
+    *     FIRST STAGE
+    *
+    *          - DETERMINE OIL RIG ON FIRE
+    *          - DETERMINE PATH
+    *
+    **************************************************/
+
     //TODO: EXTEND ARM AND FACE OILRIGS
-    
+
     //OILRIG 1 DISTANCE READING
 
     //TODO: ROTATE ARM TO NEXT OIL RIG
-    
-    //OILRIG 2 DISTANCE READING  
-    
+
+    //OILRIG 2 DISTANCE READING
+
     //ROTATE ARM TO NEXT OIL RIG
-    
+
     //OILRIG 3 DISTANCE READING
 
 }
@@ -113,64 +160,84 @@
 
 **************/
 
-void setServoPulse(uint8_t n, float pulse) {
+void setServoPulse(uint8_t n, int angle)
+{
+    int pulse;
+    pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE -  MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
     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); 
-          }
+    pc.printf("ServoNumber: %d  Begining Pulse: %d\n\r",n,currentPosition[n]);
+    int pulse2;
+    if(currentPosition[n] < pulse) {
+        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);
-           }
+        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);  
+    pc.printf("\nEND: pulse: %d,  angle: %d\n\n", i, angle);
 }
 
-void initServoDriver(void) {
+void initServoDriver(void)
+{
     pwm.begin();
     //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 servoBegin(void)
+{
+    pc.printf("Setting Initial Position\n\r");
+    setServoPulseNo_delay(0, 90);
+    setServoPulseNo_delay(1, 90);
+    setServoPulseNo_delay(2, 90);
+    setServoPulseNo_delay(3, 90);
+    setServoPulseNo_delay(4, 90);
+    setServoPulseNo_delay(5, 90);
+    setGripper(1);
 }
 
-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); 
-    }
-    
-void setServoPulseNo_delay(uint8_t n, float pulse) {
+void setServoPulseNo_delay(uint8_t n, int angle)
+{
+    int pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE -  MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
     float pulselength = 20000;   // 20,000 us per second
     currentPosition[n] = pulse;
+    pc.printf("ServoNumber: %d    Pulsewidth: %d  Angle: %d \n\r",n,pulse, angle);
     pulse = 4094 * pulse / pulselength;
     pwm.setPWM(n, 0, pulse);
+
 }
-    
-void servoPosition(int set){   
-            //moves to current position
-            setServoPulse(0, Arm_Table[set].base_rotate);
-            setServoPulse(1, Arm_Table[set].base_arm);
-            setServoPulse(2, Arm_Table[set].lil_arm);
-            setServoPulse(3, Arm_Table[set].big_arm);
-            setServoPulse(4, Arm_Table[set].claw_arm); 
-            setServoPulse(5, Arm_Table[set].claw_rotate); 
-            setServoPulse(6, Arm_Table[set].claw_open); 
+
+void setGripper(int open)
+{
+    if (open) {
+        pc.printf("Gripper Open\r");
+        setServoPulseNo_delay(6, 10);
+    } else {
+        pc.printf("Gripper  Closed\n\r");
+        setServoPulseNo_delay(6, 170);
+    }
+}
+
+void servoPosition(int set)
+{
+    //moves to current position
+    setServoPulse(0, Arm_Table[set].base_rotate);
+    setServoPulse(1, Arm_Table[set].base_arm);
+    setServoPulse(2, Arm_Table[set].lil_arm);
+    setServoPulse(3, Arm_Table[set].big_arm);
+    setServoPulse(4, Arm_Table[set].claw_arm);
+    setServoPulse(5, Arm_Table[set].claw_rotate);
+    setServoPulse(6, Arm_Table[set].claw_open);
 }