Diff: main.cpp

Revision:

0:1b64a0cedc5d

Child:

1:fe4a0b47ff25

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Mar 03 15:27:32 2014 +0000
@@ -0,0 +1,143 @@
+#include "mbed.h"
+#include "Adafruit_PWMServoDriver.h"
+#include "ShapeDetect.h"
+
+Serial pc(USBTX,USBRX);
+Adafruit_PWMServoDriver pwm(p9,p10);
+DigitalOut ServoOutputDisable(p8);
+extern Serial lrf;
+
+
+
+//Servo Positions
+#define STORE_POSITION  0
+#define OIL_RIG1        1
+#define OIL_RIG2        2
+
+void restingState(void);
+void initServoDriver(void);
+void setServoPulse(uint8_t n, float pulse);
+void servoPosition(int set);
+
+/************
+Variables for Servos
+*************/
+int servoNum, pulseWidth, outputDisabled, posNum;
+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, 2300, 500, 600, 2450, 2450, 0, 0}, // storing position
+    {OIL_RIG2, 1200, 1400, 1900, 900, 900, 0, 0} // point laser at oilrig2
+
+}; 
+
+int main() {
+    
+/*****************
+ INITIALIZATIONS
+*******************/
+//lrf_baudCalibration();
+initServoDriver();
+restingState();
+servoPosition(STORE_POSITION);
+ServoOutputDisable = 0;
+while(1){
+        if(pc.readable()){
+            pc.scanf("%d %d %d", &outputDisabled, &servoNum, &pulseWidth);
+            //pc.scanf("%d", &posNum);
+            //servoPosition(posNum);
+             setServoPulse(servoNum, pulseWidth);
+             ServoOutputDisable = outputDisabled;
+        }
+}
+
+
+/**************************************************
+*     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  
+    
+    //ROTATE ARM TO NEXT OIL RIG
+    
+    //OILRIG 3 DISTANCE READING
+
+}
+
+
+
+/************
+
+Servo Functions
+
+**************/
+
+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);        
+}
+
+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 restingState(void){
+    /*
+    currentPosition[0] = 2300;
+    currentPosition[1] = 500;
+    currentPosition[2] = 600;
+    currentPosition[3] = 2450;
+    currentPosition[4] = 2450;
+    currentPosition[5] = 0;
+    currentPosition[6] = 0;
+    */
+    
+    setServoPulse(0, 2300);
+    setServoPulse(1, 500);
+    setServoPulse(2, 600);
+    setServoPulse(3, 2450);
+    setServoPulse(4, 2450); 
+    }
+    
+void servoPosition(int set){   
+            //moves to current position
+            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);
+            setServoPulse(4, Arm_Table[set].claw_arm); 
+            setServoPulse(5, Arm_Table[set].claw_rotate); 
+            setServoPulse(6, Arm_Table[set].claw_open); 
+}
+
+
+