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@0:1b64a0cedc5d, 2014-03-03 (annotated)

Committer:
tashworth
Date:
Mon Mar 03 15:27:32 2014 +0000
Revision:
0:1b64a0cedc5d
Child:
1:fe4a0b47ff25
servo fix, and servo positions

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 0:1b64a0cedc5d 16
tashworth 0:1b64a0cedc5d 17 void restingState(void);
tashworth 0:1b64a0cedc5d 18 void initServoDriver(void);
tashworth 0:1b64a0cedc5d 19 void setServoPulse(uint8_t n, float pulse);
tashworth 0:1b64a0cedc5d 20 void servoPosition(int set);
tashworth 0:1b64a0cedc5d 21
tashworth 0:1b64a0cedc5d 22 /************
tashworth 0:1b64a0cedc5d 23 Variables for Servos
tashworth 0:1b64a0cedc5d 24 *************/
tashworth 0:1b64a0cedc5d 25 int servoNum, pulseWidth, outputDisabled, posNum;
tashworth 0:1b64a0cedc5d 26 int currentPosition[7];
tashworth 0:1b64a0cedc5d 27
tashworth 0:1b64a0cedc5d 28 typedef struct {int arm_action; int base_rotate; int base_arm;
tashworth 0:1b64a0cedc5d 29 int lil_arm; int big_arm; int claw_arm;
tashworth 0:1b64a0cedc5d 30 int claw_rotate; int claw_open;} Coord;
tashworth 0:1b64a0cedc5d 31
tashworth 0:1b64a0cedc5d 32 Coord Arm_Table[] =
tashworth 0:1b64a0cedc5d 33 {
tashworth 0:1b64a0cedc5d 34 // POSITION ODER:
tashworth 0:1b64a0cedc5d 35 // base_rotate, base_arm, lil_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open
tashworth 0:1b64a0cedc5d 36
tashworth 0:1b64a0cedc5d 37 {STORE_POSITION, 2300, 500, 600, 2450, 2450, 0, 0}, // storing position
tashworth 0:1b64a0cedc5d 38 {OIL_RIG2, 1200, 1400, 1900, 900, 900, 0, 0} // point laser at oilrig2
tashworth 0:1b64a0cedc5d 39
tashworth 0:1b64a0cedc5d 40 };
tashworth 0:1b64a0cedc5d 41
tashworth 0:1b64a0cedc5d 42 int main() {
tashworth 0:1b64a0cedc5d 43
tashworth 0:1b64a0cedc5d 44 /*****************
tashworth 0:1b64a0cedc5d 45 INITIALIZATIONS
tashworth 0:1b64a0cedc5d 46 *******************/
tashworth 0:1b64a0cedc5d 47 //lrf_baudCalibration();
tashworth 0:1b64a0cedc5d 48 initServoDriver();
tashworth 0:1b64a0cedc5d 49 restingState();
tashworth 0:1b64a0cedc5d 50 servoPosition(STORE_POSITION);
tashworth 0:1b64a0cedc5d 51 ServoOutputDisable = 0;
tashworth 0:1b64a0cedc5d 52 while(1){
tashworth 0:1b64a0cedc5d 53 if(pc.readable()){
tashworth 0:1b64a0cedc5d 54 pc.scanf("%d %d %d", &outputDisabled, &servoNum, &pulseWidth);
tashworth 0:1b64a0cedc5d 55 //pc.scanf("%d", &posNum);
tashworth 0:1b64a0cedc5d 56 //servoPosition(posNum);
tashworth 0:1b64a0cedc5d 57 setServoPulse(servoNum, pulseWidth);
tashworth 0:1b64a0cedc5d 58 ServoOutputDisable = outputDisabled;
tashworth 0:1b64a0cedc5d 59 }
tashworth 0:1b64a0cedc5d 60 }
tashworth 0:1b64a0cedc5d 61
tashworth 0:1b64a0cedc5d 62
tashworth 0:1b64a0cedc5d 63 /**************************************************
tashworth 0:1b64a0cedc5d 64 * FIRST STAGE
tashworth 0:1b64a0cedc5d 65 *
tashworth 0:1b64a0cedc5d 66 * - DETERMINE OIL RIG ON FIRE
tashworth 0:1b64a0cedc5d 67 * - DETERMINE PATH
tashworth 0:1b64a0cedc5d 68 *
tashworth 0:1b64a0cedc5d 69 **************************************************/
tashworth 0:1b64a0cedc5d 70
tashworth 0:1b64a0cedc5d 71 //TODO: EXTEND ARM AND FACE OILRIGS
tashworth 0:1b64a0cedc5d 72
tashworth 0:1b64a0cedc5d 73 //OILRIG 1 DISTANCE READING
tashworth 0:1b64a0cedc5d 74
tashworth 0:1b64a0cedc5d 75 //TODO: ROTATE ARM TO NEXT OIL RIG
tashworth 0:1b64a0cedc5d 76
tashworth 0:1b64a0cedc5d 77 //OILRIG 2 DISTANCE READING
tashworth 0:1b64a0cedc5d 78
tashworth 0:1b64a0cedc5d 79 //ROTATE ARM TO NEXT OIL RIG
tashworth 0:1b64a0cedc5d 80
tashworth 0:1b64a0cedc5d 81 //OILRIG 3 DISTANCE READING
tashworth 0:1b64a0cedc5d 82
tashworth 0:1b64a0cedc5d 83 }
tashworth 0:1b64a0cedc5d 84
tashworth 0:1b64a0cedc5d 85
tashworth 0:1b64a0cedc5d 86
tashworth 0:1b64a0cedc5d 87 /************
tashworth 0:1b64a0cedc5d 88
tashworth 0:1b64a0cedc5d 89 Servo Functions
tashworth 0:1b64a0cedc5d 90
tashworth 0:1b64a0cedc5d 91 **************/
tashworth 0:1b64a0cedc5d 92
tashworth 0:1b64a0cedc5d 93 void setServoPulse(uint8_t n, float pulse) {
tashworth 0:1b64a0cedc5d 94 /* float pulselength = 20000; // 20,000 us per second
tashworth 0:1b64a0cedc5d 95 if (currentPosition[n] < pulse){
tashworth 0:1b64a0cedc5d 96 for (int i=currentPosition[n]; i<(pulse+1); i++)
tashworth 0:1b64a0cedc5d 97 pulse = 4094 * currentPosition[n]+1 / pulselength;
tashworth 0:1b64a0cedc5d 98 pwm.setPWM(n, 0, pulse);
tashworth 0:1b64a0cedc5d 99 } else {
tashworth 0:1b64a0cedc5d 100 }*/
tashworth 0:1b64a0cedc5d 101 float pulselength = 20000; // 20,000 us per second
tashworth 0:1b64a0cedc5d 102 pulse = 4094 * currentPosition[n]+1 / pulselength;
tashworth 0:1b64a0cedc5d 103 pwm.setPWM(n, 0, pulse);
tashworth 0:1b64a0cedc5d 104 }
tashworth 0:1b64a0cedc5d 105
tashworth 0:1b64a0cedc5d 106 void initServoDriver(void) {
tashworth 0:1b64a0cedc5d 107 pwm.begin();
tashworth 0:1b64a0cedc5d 108 //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale().
tashworth 0:1b64a0cedc5d 109 pwm.setPrescale(140); //This value is decided for 20ms interval.
tashworth 0:1b64a0cedc5d 110 pwm.setI2Cfreq(400000); //400kHz
tashworth 0:1b64a0cedc5d 111 }
tashworth 0:1b64a0cedc5d 112
tashworth 0:1b64a0cedc5d 113 void restingState(void){
tashworth 0:1b64a0cedc5d 114 /*
tashworth 0:1b64a0cedc5d 115 currentPosition[0] = 2300;
tashworth 0:1b64a0cedc5d 116 currentPosition[1] = 500;
tashworth 0:1b64a0cedc5d 117 currentPosition[2] = 600;
tashworth 0:1b64a0cedc5d 118 currentPosition[3] = 2450;
tashworth 0:1b64a0cedc5d 119 currentPosition[4] = 2450;
tashworth 0:1b64a0cedc5d 120 currentPosition[5] = 0;
tashworth 0:1b64a0cedc5d 121 currentPosition[6] = 0;
tashworth 0:1b64a0cedc5d 122 */
tashworth 0:1b64a0cedc5d 123
tashworth 0:1b64a0cedc5d 124 setServoPulse(0, 2300);
tashworth 0:1b64a0cedc5d 125 setServoPulse(1, 500);
tashworth 0:1b64a0cedc5d 126 setServoPulse(2, 600);
tashworth 0:1b64a0cedc5d 127 setServoPulse(3, 2450);
tashworth 0:1b64a0cedc5d 128 setServoPulse(4, 2450);
tashworth 0:1b64a0cedc5d 129 }
tashworth 0:1b64a0cedc5d 130
tashworth 0:1b64a0cedc5d 131 void servoPosition(int set){
tashworth 0:1b64a0cedc5d 132 //moves to current position
tashworth 0:1b64a0cedc5d 133 setServoPulse(1, Arm_Table[set].base_arm,);
tashworth 0:1b64a0cedc5d 134 setServoPulse(0, Arm_Table[set].base_rotate);
tashworth 0:1b64a0cedc5d 135 setServoPulse(2, Arm_Table[set].lil_arm);
tashworth 0:1b64a0cedc5d 136 setServoPulse(3, Arm_Table[set].big_arm);
tashworth 0:1b64a0cedc5d 137 setServoPulse(4, Arm_Table[set].claw_arm);
tashworth 0:1b64a0cedc5d 138 setServoPulse(5, Arm_Table[set].claw_rotate);
tashworth 0:1b64a0cedc5d 139 setServoPulse(6, Arm_Table[set].claw_open);
tashworth 0:1b64a0cedc5d 140 }
tashworth 0:1b64a0cedc5d 141
tashworth 0:1b64a0cedc5d 142
tashworth 0:1b64a0cedc5d 143