uyvug
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
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); +} + + +