nav fixed
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
main.cpp
- Committer:
- tashworth
- Date:
- 2014-03-03
- Revision:
- 0:1b64a0cedc5d
- Child:
- 1:fe4a0b47ff25
File content as of revision 0:1b64a0cedc5d:
#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); }