For IEEE Robotics
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
main.cpp
- Committer:
- tashworth
- Date:
- 2014-03-14
- Revision:
- 5:429e9a998bab
- Parent:
- 4:42460f387701
- Child:
- 6:75259c3306dd
File content as of revision 5:429e9a998bab:
#include "mbed.h" #include "Adafruit_PWMServoDriver.h" #include "ShapeDetect.h" Serial pc(USBTX,USBRX); Adafruit_PWMServoDriver pwm(p28,p27); DigitalOut ServoOutputDisable(p8); extern Serial lrf; //Servo Positions #define STORE_POSITION 0 #define OIL_RIG1 1 #define OIL_RIG2 2 #define OIL_RIG3 3 #define IMG_SHAPE_1 4 #define IMG_SHAPE_2 5 #define IMG_SHAPE_3 6 #define GRASP_SHAPE_1 7 #define GRASP_SHAPE_2 8 #define GRASP_SHAPE_3 9 #define INSERT_TOOL_1 10 #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, int angle); void setServoPulseNo_delay(uint8_t n, int angle); void servoPosition(int set); void setGripper(int open); /************ Variables for Servos *************/ 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, 90, 0, 177, 0, 0, 90, 90}, // storing position {OIL_RIG1, 90, 60, 130, 75, 60, 90, 90}, // point laser at oilrig1 {OIL_RIG2, 120, 60, 130, 75, 60, 90, 90}, // point laser at oilrig2 {OIL_RIG3, 135, 60, 130, 75, 60, 90, 90}, // point laser at oilrig2 }; /* Variables for imageprocessing and distance */ int x_coord; int y_coord; int area; int shape; int main() { /***************** INITIALIZATIONS *******************/ pc.baud(115200); //Laser Range Finder Initialization lrf_baudCalibration(); //Servo initialization initServoDriver(); servoBegin(); // initiates servos to start position //ServoOutputDisable = 0; while(1){ //pc.scanf("%d %d", &servoNum, &servoAngle); //setServoPulse(servoNum, servoAngle); pc.scanf("%d", &posNum); servoPosition(posNum); } /* 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); 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; }*/ /*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 * **************************************************/ //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, 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("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) { for(i; i > pulse; i--) { pulse2 = 4094 * i / pulselength; pwm.setPWM(n, 0, pulse2); wait_ms(3); } } currentPosition[n] = i; pc.printf("\nEND: pulse: %d, angle: %d\n\n", i, angle); } 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, 0); setServoPulseNo_delay(2, 177); setServoPulseNo_delay(3, 0); setServoPulseNo_delay(4, 0); setServoPulseNo_delay(5, 90); setGripper(1); } 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 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); }