Peihsun Yeh
CarlsenBot working
Diff: main.cpp
- Revision:
- 0:cdd566529530
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed May 01 16:00:53 2013 +0000
@@ -0,0 +1,390 @@
+#include "mbed.h"
+#include "Motor.h"
+#include "Servo.h"
+#include "string.h"
+#include <iostream>
+
+// Open: 0 max open
+#define OPEN 0.43
+// Close 1 max close **Don't set to 1
+#define CLOSE 0.87
+// absolute positions for the z-axis
+#define UP 0.36
+#define DOWN 0.512
+
+// Object declarations
+I2C i2c(p9, p10);
+Serial pc(USBTX, USBRX);
+Motor motor_x1(p21, p5, p6, &i2c);
+Motor motor_x2(p22, p11, p12, &i2c);
+Motor motor_y(p23, p15, p16, &i2c);
+Motor motor_z(p24, p13, p14);
+Servo claw(p25);
+AnalogIn z_dist(p20);
+Serial device(p28, p27); // tx, rx
+Ticker pos_monitor;
+DigitalOut leds[] = { (LED1), (LED2), (LED3), (LED4) };
+
+const int XWIDTH = 208;
+const int YWIDTH = 554;
+char resetAll = 0x4E; // to reset all devices on one i2c bus, write data 0x4E to address 0x00.
+float height;
+
+//function declarations
+void send_char( char* );
+void getAllMeasurements(void);
+void moveX(int pos);
+void moveY(int pos);
+void moveZ(float pos);
+void movePiece(char c1, char r1, char c2, char r2);
+
+int main()
+{
+ // initializing routine for all i2c devices
+ // no motor_z.init since it it not a i2c device
+ // since motor_x2's orientation is switched, flip "is_reversed" bit
+ i2c.write(0x00, &resetAll, 1);
+ motor_x1.init(0x20, 1, false);
+ motor_x2.init(0x22, 2, false);
+ motor_y.init(0x24, 3, true);
+ motor_x2.is_reversed = true;
+
+ /* for debugging
+ motor_x1.getTicks();
+ motor_x2.getTicks();
+ motor_y.getTicks();
+ */
+
+ pos_monitor.attach(&getAllMeasurements, 0.003); // interrupt routine to measure current position on all axis
+
+ // get claw ready
+ // note to self: in future revisions, implement a way to tare X and Y
+ moveZ(UP);
+ claw = OPEN;
+
+ //main variables
+ char char_buf[4];
+ char rchar = 'z';
+ char status = '0';
+ int count = 0;
+ int numLeds = sizeof(leds)/sizeof(DigitalOut);
+
+ // wake up device
+ // done by sending EasyVR 'b' character
+ // recommended wake routine: keep sending 'b' until confirmation, via 'o'
+ device.putc('b');
+ while (device.getc()!='o') {
+ device.putc('b');
+ leds[0] = 1;
+ wait(0.2);
+ }
+
+//reset all leds initially
+ for(int i = 0; i < numLeds; i++) {
+ leds[i] = 0;
+ }
+
+//wait until ready
+ while (device.readable()!=0) {}
+
+ //movePiece('b', '2', 'b', '4');
+
+//main loop
+ while (1) {
+ leds[0] = 0;
+ leds[1] = 0;
+ leds[2] = 0;
+ leds[3] = 1; //solid led4 means mbed is in wait for request mode
+ while (pc.readable() == 0) {} //wait for a readable byte
+ status = pc.getc();
+
+ if(status == 'n') {
+ // REQUEST FOR VR COMMAND
+ while ( count < 4 ) {
+ //flash led 4 for user confirmation of new recog
+ leds[3] = !leds[3];
+ //start recog based off either words or numbers
+ if(count == 0 || count == 2) {
+ device.putc('d');
+ wait(0.001);
+ device.putc('C'); //Ben's trained words (C) // FLOHRS (B)
+ wait(0.001);
+ //pc.printf("Say a column letter\n");
+ } else if(count == 1|| count == 3) {
+ device.putc('i');
+ wait(0.001);
+ device.putc('D'); //Wordset #3 - numbers
+ wait(0.001);
+ //pc.printf("Say a row number\n");
+ }
+ //wait for confirmation of new recog
+ while (device.readable()!=0) {}
+ rchar=device.getc();
+
+ // Word recognized
+ // Each move is a sequence of 4 words. Each word recognized makes an LED light up
+ if (rchar=='r') {
+ wait(.001);
+ device.putc(' ');
+ while (device.readable()!=0) {}
+ rchar=device.getc();
+ //pc.printf("Recognized:");
+ //pc.putc(rchar-'A'+'a');
+ char_buf[count] = (rchar-'A'+'a');
+ leds[count] = 1;
+ count++;
+ // error
+ } else if (rchar=='s') {
+ wait(.001);
+ device.putc(' ');
+ while (device.readable()!=0) {}
+ rchar=device.getc();
+ //pc.printf("Recognized:");
+ //pc.putc(rchar-'A'+'0');
+ char_buf[count] = (rchar-'A'+'0');
+ leds[count] = 1;
+ count++;
+ } else if (rchar=='e') {
+ wait(.001);
+ device.putc(' ');
+ while (device.readable()!=0) {}
+ rchar=device.getc();
+ device.putc(' ');
+ while (device.readable()!=0) {}
+ rchar=device.getc();
+ }
+ }
+ //successful input string created
+
+ //reset all leds
+ for(int i = 0; i < numLeds; i++) {
+ leds[i] = 0;
+ }
+ count = 0;
+ //send data to EEBOX
+ send_char(char_buf);
+ } else if( status == 'm') {
+ leds[0] = 0;
+ leds[1] = 1;
+ leds[2] = 0;
+ leds[3] = 1;
+ //status for claw move
+ //wait for reply
+ pc.putc('m');
+ while( pc.readable() == 0);
+ count = 0;
+ char_buf[count] = pc.getc(); //grab char
+ leds[count] = 1;
+ count++;
+ leds[0] = 0;
+ leds[1] = 0;
+ leds[2] = 0;
+ leds[3] = 0;
+ while (count < 4) {
+ leds[count] = 1;
+ pc.putc(' ');
+ while (pc.readable() == 0) {} //wait for response (beginning of string)
+ char_buf[count] = pc.getc();
+ count++;
+ }
+ leds[0] = 0;
+ leds[1] = 0;
+ leds[2] = 0;
+ leds[3] = 0;
+ //Recieved the 4 char buffer for movement
+ movePiece(char_buf[0], char_buf[1], char_buf[2], char_buf[3]);
+ //wait(10);
+ //pc.printf("mbed recieved following string for move:\n");
+ //pc.printf("%c%c%c%c\n\n", char_buf[0], char_buf[1], char_buf[2], char_buf[3]);
+ count = 0;
+ }
+ }
+
+
+}
+
+//Function Definitions
+void send_char( char output[] )
+{
+ //Successful Request, send confirmation byte
+ char status;
+ do {
+ pc.putc('n');
+ while (pc.readable() == 0) {} //wait for a readable byte
+ status = pc.getc();
+ } while( status != ' ');
+
+ for(int i = 0; i < 4; i++) {
+
+ do {
+ while (pc.readable() == 0) {} //wait for a readable byte
+ status = pc.getc();
+ } while( status != ' ');
+
+ pc.putc(output[i]);
+
+ }
+
+ //terminate communication
+ do {
+ while (pc.readable() == 0) {} //wait for a readable byte
+ status = pc.getc();
+ } while( status != ' ');
+
+ pc.putc('s'); //send termination byte
+
+}
+void moveX(int pos) //make pos signed
+{
+ float diff1, diff2, spd1, spd2, lower = 0.06, upper = 0.8, integral1 = 0.0, integral2 = 0.0;
+ //int K = 1000;
+ int K = 2000, Ki = 0.001;
+ while (!((abs(motor_x1.ticks - pos) <= 9) && (abs(motor_x2.ticks - pos) <= 9))) {
+ diff1 = (pos - motor_x1.ticks);
+ diff2 = (pos - motor_x2.ticks);
+ integral1 += (diff1*0.001);
+ integral2 += (diff2*0.001);
+ spd1 = (diff1/K) + (Ki*integral1);
+ spd2 = (diff2/K) + (Ki*integral2);
+ if (spd1 >= upper) spd1 = upper;
+ else if(spd1 <= lower && spd1 > 0.0) spd1 = lower;
+ else if(spd1 < 0.0 && spd1 >= -lower) spd1 = -lower;
+ else if(spd1 <= -upper) spd1 = -upper;
+ //rationalize a speed for the motor x2
+ if (spd2 >= upper) spd2 = upper;
+ else if(spd2 <= lower && spd2 > 0.0) spd2 = lower;
+ else if(spd2 < 0.0 && spd2 >= -lower) spd2 = -lower;
+ else if(spd2 <= -upper) spd2 = -upper;
+
+ //printf("Motor %d is at position %d, moving with speed %f\n", motor_x1.motorID, motor_x1.ticks, spd1); // debugging
+ //printf("Motor %d is at position %d, moving with speed %f\n", motor_x2.motorID, motor_x2.ticks, spd2); // debugging
+
+ //adjust speed according to difference between two motors
+ if (abs(motor_x1.ticks - motor_x2.ticks) <= 10) {
+ motor_x1.speed(spd1);
+ motor_x2.speed(spd2);
+ wait(0.001);
+ }
+ //if motor x1 is lagging behind motor x2, slow motor x2
+ else if (abs(diff1) > abs(diff2)) {
+ //apply new speed settings to x motors
+ motor_x2.speed(spd2/2);
+ motor_x1.speed(spd1);
+ wait(0.001);
+ }
+ //if motor x2 is lagging behind motor x1, slow motor x1
+ else if (abs(diff2) > abs(diff1)) {
+ //apply new speed settings to x motors
+ motor_x1.speed(spd1/2);
+ motor_x2.speed(spd2);
+ wait(0.001);
+ }
+ }//while
+ motor_x1.speed(0);
+ motor_x2.speed(0);
+ wait(0.5);
+ //adjusting
+ while (!((abs(motor_x1.ticks - pos) <= 6) && (abs(motor_x2.ticks - pos) <= 6))) {
+ diff1 = (pos - motor_x1.ticks);
+ diff2 = (pos - motor_x2.ticks);
+ if(diff1 > 0) spd1 = 0.05;
+ else if(diff1 < 0) spd1 = -0.05;
+ else if(diff1 == 0) spd1 = 0;
+ if(diff2 > 0) spd2 = 0.05;
+ else if(diff2 < 0) spd2 = -0.05;
+ else if(diff2 == 0) spd2 = 0;
+ //printf("Motor %d is at position %d, moving with speed %f\n", motor_x1.motorID, motor_x1.ticks, spd1); // debugging
+ //printf("Motor %d is at position %d, moving with speed %f\n", motor_x2.motorID, motor_x2.ticks, spd2); // debugging
+
+ motor_x1.speed(spd1);
+ motor_x2.speed(spd2);
+ wait(0.005);
+ }
+ motor_x1.speed(0);
+ motor_x2.speed(0);
+ wait(0.5);
+ //printf("Done! Motor %d is at position %d\n", motor_x1.motorID, motor_x1.ticks); // debugging
+ //printf("Done! Motor %d is at position %d\n", motor_x2.motorID, motor_x2.ticks); // debugging
+}
+
+void moveZ(float pos) // ~0.31 for top, ~0.46 for bottom
+{
+ float tempPos = pos;
+ if(pos == DOWN){
+ pos = 0.47;
+ }
+ //float height1, height2;
+ while(!((abs(height - pos) < 0.01))) {
+ //pc.printf("z height: %f\n", height);
+ if(height < pos) {
+ motor_z.speed(0.025);
+ wait(0.02);
+ motor_z.speed(0.015);
+ } else if(height > pos) {
+ motor_z.speed(-0.16);
+ wait(0.02);
+ motor_z.speed(-0.08);
+ }
+ } // ~ .47
+ pos = tempPos;
+ wait(0.5);
+ while(!((abs(height - pos) < 0.01))) { // adjusting loop
+ //pc.printf("z height: %f\n", height);
+ if(height < pos) {
+ motor_z.speed(0.02);
+ wait(0.02);
+ motor_z.speed(0.01);
+ } else if(height > pos) {
+ motor_z.speed(-0.1);
+ wait(0.02);
+ motor_z.speed(-0.05);
+ }
+ }
+ motor_z.speed(0);
+}
+void getAllMeasurements()
+{
+ motor_x1.getTicks();
+ motor_x2.getTicks();
+ motor_y.getTicks();
+ height = z_dist;
+}
+
+void moveY(int pos)
+{
+ motor_y.move(pos);
+}
+
+void movePiece(char c1, char r1, char c2, char r2)
+{
+ // c1, c2 should be within 'a' and 'h'
+ // subtracting 'a' from c1 or c2 get it's "square offset" from initial position
+ // i.e. if c1 = 'b', 'b'-'a'=1 tells it to move the distance of 1 square forward
+ // r1, r2 should be within '1' and '9', or ':'
+ // same logic, but with '1'
+
+ //*** NOTE:
+ //When a move is a capture, [c2 r2] will always be [b :]
+ int goToY = (c1 - 'a')*YWIDTH;
+ int goToX = (r1 - '1')*XWIDTH;
+ moveX(goToX);
+ moveY(goToY);
+
+ // go down to pick up a piece, and go back up
+ moveZ(DOWN);
+ wait(0.7);
+ claw = CLOSE;
+ wait(0.5);
+ moveZ(UP);
+
+ // move to destination
+ goToY = (c2 - 'a')*YWIDTH;
+ goToX = (r2 - '1')*XWIDTH;
+ moveX(goToX);
+ moveY(goToY);
+ moveZ(DOWN);
+ wait(0.7);
+ claw = OPEN;
+ wait(0.5);
+ moveZ(UP);
+}
\ No newline at end of file