For IEEE Robotics
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Diff: main.cpp
- Revision:
- 3:b7b4780a7f6e
- Parent:
- 2:4e082e4c255d
- Child:
- 4:42460f387701
diff -r 4e082e4c255d -r b7b4780a7f6e main.cpp --- 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); }