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

Committer:
tashworth
Date:
Mon Mar 03 21:37:24 2014 +0000
Revision:
1:fe4a0b47ff25
Parent:
0:1b64a0cedc5d
Child:
2:4e082e4c255d
servo timing slowed down (smoother)

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 1:fe4a0b47ff25 17 void servoBegin(void);
tashworth 0:1b64a0cedc5d 18 void initServoDriver(void);
tashworth 0:1b64a0cedc5d 19 void setServoPulse(uint8_t n, float pulse);
tashworth 1:fe4a0b47ff25 20 void setServoPulseNo_delay(uint8_t n, float pulse);
tashworth 0:1b64a0cedc5d 21 void servoPosition(int set);
tashworth 0:1b64a0cedc5d 22
tashworth 0:1b64a0cedc5d 23 /************
tashworth 0:1b64a0cedc5d 24 Variables for Servos
tashworth 0:1b64a0cedc5d 25 *************/
tashworth 0:1b64a0cedc5d 26 int servoNum, pulseWidth, outputDisabled, posNum;
tashworth 0:1b64a0cedc5d 27 int currentPosition[7];
tashworth 0:1b64a0cedc5d 28
tashworth 0:1b64a0cedc5d 29 typedef struct {int arm_action; int base_rotate; int base_arm;
tashworth 0:1b64a0cedc5d 30 int lil_arm; int big_arm; int claw_arm;
tashworth 0:1b64a0cedc5d 31 int claw_rotate; int claw_open;} Coord;
tashworth 0:1b64a0cedc5d 32
tashworth 0:1b64a0cedc5d 33 Coord Arm_Table[] =
tashworth 0:1b64a0cedc5d 34 {
tashworth 0:1b64a0cedc5d 35 // POSITION ODER:
tashworth 0:1b64a0cedc5d 36 // base_rotate, base_arm, lil_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open
tashworth 0:1b64a0cedc5d 37
tashworth 0:1b64a0cedc5d 38 {STORE_POSITION, 2300, 500, 600, 2450, 2450, 0, 0}, // storing position
tashworth 0:1b64a0cedc5d 39 {OIL_RIG2, 1200, 1400, 1900, 900, 900, 0, 0} // point laser at oilrig2
tashworth 0:1b64a0cedc5d 40
tashworth 0:1b64a0cedc5d 41 };
tashworth 0:1b64a0cedc5d 42
tashworth 0:1b64a0cedc5d 43 int main() {
tashworth 0:1b64a0cedc5d 44
tashworth 0:1b64a0cedc5d 45 /*****************
tashworth 0:1b64a0cedc5d 46 INITIALIZATIONS
tashworth 0:1b64a0cedc5d 47 *******************/
tashworth 0:1b64a0cedc5d 48 //lrf_baudCalibration();
tashworth 0:1b64a0cedc5d 49 initServoDriver();
tashworth 1:fe4a0b47ff25 50 servoBegin();
tashworth 1:fe4a0b47ff25 51 //servoPosition(STORE_POSITION);
tashworth 1:fe4a0b47ff25 52 //ServoOutputDisable = 0;
tashworth 0:1b64a0cedc5d 53 while(1){
tashworth 0:1b64a0cedc5d 54 if(pc.readable()){
tashworth 1:fe4a0b47ff25 55 //pc.scanf("%d %d %d", &outputDisabled, &servoNum, &pulseWidth);
tashworth 1:fe4a0b47ff25 56 pc.scanf("%d %d", &servoNum, &pulseWidth);
tashworth 0:1b64a0cedc5d 57 setServoPulse(servoNum, pulseWidth);
tashworth 1:fe4a0b47ff25 58
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 1:fe4a0b47ff25 94 float pulselength = 20000; // 20,000 us per second
tashworth 1:fe4a0b47ff25 95 int i = currentPosition[n];
tashworth 1:fe4a0b47ff25 96 pc.printf("\ncurrent position = %d\n", currentPosition[n]);
tashworth 1:fe4a0b47ff25 97 int pulse2;
tashworth 1:fe4a0b47ff25 98 if(currentPosition[n] < pulse){
tashworth 1:fe4a0b47ff25 99 pc.printf("\ncurrent position < pulse\n");
tashworth 1:fe4a0b47ff25 100 for(i; i < pulse; i++){
tashworth 1:fe4a0b47ff25 101 pulse2 = 4094 * i / pulselength;
tashworth 1:fe4a0b47ff25 102 pwm.setPWM(n, 0, pulse2);
tashworth 1:fe4a0b47ff25 103 wait_ms(3);
tashworth 1:fe4a0b47ff25 104 }
tashworth 1:fe4a0b47ff25 105 } else if (currentPosition[n] > pulse) {
tashworth 1:fe4a0b47ff25 106 pc.printf("\ncurrent position > pulse\n");
tashworth 1:fe4a0b47ff25 107 for(i; i > pulse; i--){
tashworth 1:fe4a0b47ff25 108 pulse2 = 4094 * i / pulselength;
tashworth 1:fe4a0b47ff25 109 pwm.setPWM(n, 0, pulse2);
tashworth 1:fe4a0b47ff25 110 wait_ms(3);
tashworth 1:fe4a0b47ff25 111 }
tashworth 1:fe4a0b47ff25 112 }
tashworth 1:fe4a0b47ff25 113 currentPosition[n] = i;
tashworth 1:fe4a0b47ff25 114 pc.printf("\nending position = %d\n\n", i);
tashworth 0:1b64a0cedc5d 115 }
tashworth 0:1b64a0cedc5d 116
tashworth 0:1b64a0cedc5d 117 void initServoDriver(void) {
tashworth 0:1b64a0cedc5d 118 pwm.begin();
tashworth 0:1b64a0cedc5d 119 //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale().
tashworth 0:1b64a0cedc5d 120 pwm.setPrescale(140); //This value is decided for 20ms interval.
tashworth 0:1b64a0cedc5d 121 pwm.setI2Cfreq(400000); //400kHz
tashworth 1:fe4a0b47ff25 122
tashworth 0:1b64a0cedc5d 123 }
tashworth 0:1b64a0cedc5d 124
tashworth 1:fe4a0b47ff25 125 void servoBegin(void){
tashworth 1:fe4a0b47ff25 126 setServoPulseNo_delay(0, 900);
tashworth 1:fe4a0b47ff25 127 setServoPulseNo_delay(1, 500);
tashworth 1:fe4a0b47ff25 128 setServoPulseNo_delay(2, 600);
tashworth 1:fe4a0b47ff25 129 setServoPulseNo_delay(3, 2450);
tashworth 1:fe4a0b47ff25 130 setServoPulseNo_delay(4, 2450);
tashworth 1:fe4a0b47ff25 131 setServoPulseNo_delay(5, 0);
tashworth 1:fe4a0b47ff25 132 setServoPulseNo_delay(6, 0);
tashworth 1:fe4a0b47ff25 133 }
tashworth 0:1b64a0cedc5d 134
tashworth 1:fe4a0b47ff25 135 void setServoPulseNo_delay(uint8_t n, float pulse) {
tashworth 1:fe4a0b47ff25 136 float pulselength = 20000; // 20,000 us per second
tashworth 1:fe4a0b47ff25 137 currentPosition[n] = pulse;
tashworth 1:fe4a0b47ff25 138 pulse = 4094 * pulse / pulselength;
tashworth 1:fe4a0b47ff25 139 pwm.setPWM(n, 0, pulse);
tashworth 1:fe4a0b47ff25 140 }
tashworth 0:1b64a0cedc5d 141
tashworth 0:1b64a0cedc5d 142 void servoPosition(int set){
tashworth 0:1b64a0cedc5d 143 //moves to current position
tashworth 1:fe4a0b47ff25 144 setServoPulse(1, Arm_Table[set].base_arm);
tashworth 0:1b64a0cedc5d 145 setServoPulse(0, Arm_Table[set].base_rotate);
tashworth 0:1b64a0cedc5d 146 setServoPulse(2, Arm_Table[set].lil_arm);
tashworth 0:1b64a0cedc5d 147 setServoPulse(3, Arm_Table[set].big_arm);
tashworth 0:1b64a0cedc5d 148 setServoPulse(4, Arm_Table[set].claw_arm);
tashworth 0:1b64a0cedc5d 149 setServoPulse(5, Arm_Table[set].claw_rotate);
tashworth 0:1b64a0cedc5d 150 setServoPulse(6, Arm_Table[set].claw_open);
tashworth 0:1b64a0cedc5d 151 }
tashworth 0:1b64a0cedc5d 152
tashworth 0:1b64a0cedc5d 153
tashworth 0:1b64a0cedc5d 154