Paolina Povolotskaya / Mbed 2 deprecated theRobot

added normalization and use of keyboard. I tested it and it worked

Dependencies:   Adafruit-16-Ch-PWM-Servo-Driver mbed

Fork of theRobot by Thomas Ashworth

main.cpp@2:4e082e4c255d, 2014-03-03 (annotated)

Committer:
tashworth
Date:
Mon Mar 03 22:30:34 2014 +0000
Revision:
2:4e082e4c255d
Parent:
1:fe4a0b47ff25
Child:
3:587441455259
3-3-14

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tashworth 0:1b64a0cedc5d 1 #include "mbed.h"
tashworth 0:1b64a0cedc5d 2 #include "Adafruit_PWMServoDriver.h"
tashworth 0:1b64a0cedc5d 3 #include "ShapeDetect.h"
tashworth 0:1b64a0cedc5d 4
tashworth 0:1b64a0cedc5d 5 Serial pc(USBTX,USBRX);
tashworth 0:1b64a0cedc5d 6 Adafruit_PWMServoDriver pwm(p9,p10);
tashworth 0:1b64a0cedc5d 7 DigitalOut ServoOutputDisable(p8);
tashworth 0:1b64a0cedc5d 8 extern Serial lrf;
tashworth 0:1b64a0cedc5d 9
tashworth 0:1b64a0cedc5d 10
tashworth 0:1b64a0cedc5d 11
tashworth 0:1b64a0cedc5d 12 //Servo Positions
tashworth 0:1b64a0cedc5d 13 #define STORE_POSITION 0
tashworth 0:1b64a0cedc5d 14 #define OIL_RIG1 1
tashworth 0:1b64a0cedc5d 15 #define OIL_RIG2 2
tashworth 2:4e082e4c255d 16 #define OIL_RIG3 3
tashworth 2:4e082e4c255d 17 #define IMG_SHAPE_1 4
tashworth 2:4e082e4c255d 18 #define IMG_SHAPE_2 5
tashworth 2:4e082e4c255d 19 #define IMG_SHAPE_3 6
tashworth 2:4e082e4c255d 20 #define GRASP_SHAPE_1 7
tashworth 2:4e082e4c255d 21 #define GRASP_SHAPE_2 8
tashworth 2:4e082e4c255d 22 #define GRASP_SHAPE_3 9
tashworth 2:4e082e4c255d 23 #define INSERT_TOOL_1 10
tashworth 2:4e082e4c255d 24 #define INSERT_TOOL_2 11
tashworth 2:4e082e4c255d 25 #define INSERT_TOOL_3 11
tashworth 2:4e082e4c255d 26
tashworth 0:1b64a0cedc5d 27
tashworth 1:fe4a0b47ff25 28 void servoBegin(void);
tashworth 0:1b64a0cedc5d 29 void initServoDriver(void);
tashworth 0:1b64a0cedc5d 30 void setServoPulse(uint8_t n, float pulse);
tashworth 1:fe4a0b47ff25 31 void setServoPulseNo_delay(uint8_t n, float pulse);
tashworth 0:1b64a0cedc5d 32 void servoPosition(int set);
tashworth 0:1b64a0cedc5d 33
tashworth 0:1b64a0cedc5d 34 /************
tashworth 0:1b64a0cedc5d 35 Variables for Servos
tashworth 0:1b64a0cedc5d 36 *************/
tashworth 0:1b64a0cedc5d 37 int servoNum, pulseWidth, outputDisabled, posNum;
tashworth 0:1b64a0cedc5d 38 int currentPosition[7];
tashworth 0:1b64a0cedc5d 39
tashworth 0:1b64a0cedc5d 40 typedef struct {int arm_action; int base_rotate; int base_arm;
tashworth 0:1b64a0cedc5d 41 int lil_arm; int big_arm; int claw_arm;
tashworth 0:1b64a0cedc5d 42 int claw_rotate; int claw_open;} Coord;
tashworth 0:1b64a0cedc5d 43
tashworth 0:1b64a0cedc5d 44 Coord Arm_Table[] =
tashworth 0:1b64a0cedc5d 45 {
tashworth 0:1b64a0cedc5d 46 // POSITION ODER:
tashworth 0:1b64a0cedc5d 47 // base_rotate, base_arm, lil_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open
tashworth 0:1b64a0cedc5d 48
tashworth 2:4e082e4c255d 49 {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
tashworth 2:4e082e4c255d 50 {OIL_RIG1, 1500, 1400, 1900, 900, 900, 0, 0}, // point laser at oilrig2
tashworth 2:4e082e4c255d 51 {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
tashworth 2:4e082e4c255d 52 {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
tashworth 2:4e082e4c255d 53 {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
tashworth 2:4e082e4c255d 54 {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
tashworth 2:4e082e4c255d 55 {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
tashworth 0:1b64a0cedc5d 56
tashworth 0:1b64a0cedc5d 57 };
tashworth 0:1b64a0cedc5d 58
tashworth 0:1b64a0cedc5d 59 int main() {
tashworth 0:1b64a0cedc5d 60
tashworth 0:1b64a0cedc5d 61 /*****************
tashworth 0:1b64a0cedc5d 62 INITIALIZATIONS
tashworth 0:1b64a0cedc5d 63 *******************/
tashworth 2:4e082e4c255d 64 //pc.baud(115200);
tashworth 2:4e082e4c255d 65 //Laser Range Finder Initialization
tashworth 0:1b64a0cedc5d 66 //lrf_baudCalibration();
tashworth 2:4e082e4c255d 67
tashworth 2:4e082e4c255d 68 //Servo initialization
tashworth 0:1b64a0cedc5d 69 initServoDriver();
tashworth 2:4e082e4c255d 70 servoBegin(); // initiates servos to start position
tashworth 2:4e082e4c255d 71 ServoOutputDisable = 0;
tashworth 2:4e082e4c255d 72
tashworth 0:1b64a0cedc5d 73 while(1){
tashworth 0:1b64a0cedc5d 74 if(pc.readable()){
tashworth 2:4e082e4c255d 75 /*
tashworth 2:4e082e4c255d 76 pc.scanf("%d %d %d", &outputDisabled, &servoNum, &pulseWidth);
tashworth 0:1b64a0cedc5d 77 setServoPulse(servoNum, pulseWidth);
tashworth 2:4e082e4c255d 78 */
tashworth 2:4e082e4c255d 79 pc.scanf("%d", &posNum);
tashworth 2:4e082e4c255d 80 servoPosition(posNum);
tashworth 2:4e082e4c255d 81
tashworth 1:fe4a0b47ff25 82
tashworth 0:1b64a0cedc5d 83 }
tashworth 0:1b64a0cedc5d 84 }
tashworth 0:1b64a0cedc5d 85
tashworth 0:1b64a0cedc5d 86 /**************************************************
tashworth 0:1b64a0cedc5d 87 * FIRST STAGE
tashworth 0:1b64a0cedc5d 88 *
tashworth 0:1b64a0cedc5d 89 * - DETERMINE OIL RIG ON FIRE
tashworth 0:1b64a0cedc5d 90 * - DETERMINE PATH
tashworth 0:1b64a0cedc5d 91 *
tashworth 0:1b64a0cedc5d 92 **************************************************/
tashworth 0:1b64a0cedc5d 93
tashworth 0:1b64a0cedc5d 94 //TODO: EXTEND ARM AND FACE OILRIGS
tashworth 0:1b64a0cedc5d 95
tashworth 0:1b64a0cedc5d 96 //OILRIG 1 DISTANCE READING
tashworth 0:1b64a0cedc5d 97
tashworth 0:1b64a0cedc5d 98 //TODO: ROTATE ARM TO NEXT OIL RIG
tashworth 0:1b64a0cedc5d 99
tashworth 0:1b64a0cedc5d 100 //OILRIG 2 DISTANCE READING
tashworth 0:1b64a0cedc5d 101
tashworth 0:1b64a0cedc5d 102 //ROTATE ARM TO NEXT OIL RIG
tashworth 0:1b64a0cedc5d 103
tashworth 0:1b64a0cedc5d 104 //OILRIG 3 DISTANCE READING
tashworth 0:1b64a0cedc5d 105
tashworth 0:1b64a0cedc5d 106 }
tashworth 0:1b64a0cedc5d 107
tashworth 0:1b64a0cedc5d 108
tashworth 0:1b64a0cedc5d 109
tashworth 0:1b64a0cedc5d 110 /************
tashworth 0:1b64a0cedc5d 111
tashworth 0:1b64a0cedc5d 112 Servo Functions
tashworth 0:1b64a0cedc5d 113
tashworth 0:1b64a0cedc5d 114 **************/
tashworth 0:1b64a0cedc5d 115
tashworth 0:1b64a0cedc5d 116 void setServoPulse(uint8_t n, float pulse) {
tashworth 1:fe4a0b47ff25 117 float pulselength = 20000; // 20,000 us per second
tashworth 1:fe4a0b47ff25 118 int i = currentPosition[n];
tashworth 1:fe4a0b47ff25 119 pc.printf("\ncurrent position = %d\n", currentPosition[n]);
tashworth 1:fe4a0b47ff25 120 int pulse2;
tashworth 1:fe4a0b47ff25 121 if(currentPosition[n] < pulse){
tashworth 1:fe4a0b47ff25 122 pc.printf("\ncurrent position < pulse\n");
tashworth 1:fe4a0b47ff25 123 for(i; i < pulse; i++){
tashworth 1:fe4a0b47ff25 124 pulse2 = 4094 * i / pulselength;
tashworth 1:fe4a0b47ff25 125 pwm.setPWM(n, 0, pulse2);
tashworth 1:fe4a0b47ff25 126 wait_ms(3);
tashworth 1:fe4a0b47ff25 127 }
tashworth 1:fe4a0b47ff25 128 } else if (currentPosition[n] > pulse) {
tashworth 1:fe4a0b47ff25 129 pc.printf("\ncurrent position > pulse\n");
tashworth 1:fe4a0b47ff25 130 for(i; i > pulse; i--){
tashworth 1:fe4a0b47ff25 131 pulse2 = 4094 * i / pulselength;
tashworth 1:fe4a0b47ff25 132 pwm.setPWM(n, 0, pulse2);
tashworth 1:fe4a0b47ff25 133 wait_ms(3);
tashworth 1:fe4a0b47ff25 134 }
tashworth 1:fe4a0b47ff25 135 }
tashworth 1:fe4a0b47ff25 136 currentPosition[n] = i;
tashworth 1:fe4a0b47ff25 137 pc.printf("\nending position = %d\n\n", i);
tashworth 0:1b64a0cedc5d 138 }
tashworth 0:1b64a0cedc5d 139
tashworth 0:1b64a0cedc5d 140 void initServoDriver(void) {
tashworth 0:1b64a0cedc5d 141 pwm.begin();
tashworth 0:1b64a0cedc5d 142 //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale().
tashworth 0:1b64a0cedc5d 143 pwm.setPrescale(140); //This value is decided for 20ms interval.
tashworth 0:1b64a0cedc5d 144 pwm.setI2Cfreq(400000); //400kHz
tashworth 1:fe4a0b47ff25 145
tashworth 0:1b64a0cedc5d 146 }
tashworth 0:1b64a0cedc5d 147
tashworth 1:fe4a0b47ff25 148 void servoBegin(void){
tashworth 1:fe4a0b47ff25 149 setServoPulseNo_delay(0, 900);
tashworth 1:fe4a0b47ff25 150 setServoPulseNo_delay(1, 500);
tashworth 1:fe4a0b47ff25 151 setServoPulseNo_delay(2, 600);
tashworth 1:fe4a0b47ff25 152 setServoPulseNo_delay(3, 2450);
tashworth 1:fe4a0b47ff25 153 setServoPulseNo_delay(4, 2450);
tashworth 1:fe4a0b47ff25 154 setServoPulseNo_delay(5, 0);
tashworth 1:fe4a0b47ff25 155 setServoPulseNo_delay(6, 0);
tashworth 1:fe4a0b47ff25 156 }
tashworth 0:1b64a0cedc5d 157
tashworth 1:fe4a0b47ff25 158 void setServoPulseNo_delay(uint8_t n, float pulse) {
tashworth 1:fe4a0b47ff25 159 float pulselength = 20000; // 20,000 us per second
tashworth 1:fe4a0b47ff25 160 currentPosition[n] = pulse;
tashworth 1:fe4a0b47ff25 161 pulse = 4094 * pulse / pulselength;
tashworth 1:fe4a0b47ff25 162 pwm.setPWM(n, 0, pulse);
tashworth 1:fe4a0b47ff25 163 }
tashworth 0:1b64a0cedc5d 164
tashworth 0:1b64a0cedc5d 165 void servoPosition(int set){
tashworth 0:1b64a0cedc5d 166 //moves to current position
tashworth 2:4e082e4c255d 167 setServoPulse(0, Arm_Table[set].base_rotate);
tashworth 1:fe4a0b47ff25 168 setServoPulse(1, Arm_Table[set].base_arm);
tashworth 0:1b64a0cedc5d 169 setServoPulse(2, Arm_Table[set].lil_arm);
tashworth 0:1b64a0cedc5d 170 setServoPulse(3, Arm_Table[set].big_arm);
tashworth 0:1b64a0cedc5d 171 setServoPulse(4, Arm_Table[set].claw_arm);
tashworth 0:1b64a0cedc5d 172 setServoPulse(5, Arm_Table[set].claw_rotate);
tashworth 0:1b64a0cedc5d 173 setServoPulse(6, Arm_Table[set].claw_open);
tashworth 0:1b64a0cedc5d 174 }
tashworth 0:1b64a0cedc5d 175
tashworth 0:1b64a0cedc5d 176
tashworth 0:1b64a0cedc5d 177