scribe robot working with stepper motors
Dependencies: BLE_nRF8001 BNO055 HC_SR04_Ultrasonic_Library localization mbed-rtos mbed
Diff: main.cpp
- Revision:
- 6:ca112c3083bb
- Parent:
- 5:2c196f871096
--- a/main.cpp Wed May 04 23:51:57 2016 +0000 +++ b/main.cpp Thu May 12 06:12:45 2016 +0000 @@ -24,6 +24,12 @@ DigitalInOut BLE_RDY(p21); DigitalInOut BLE_REQ(p22); DigitalInOut BLE_RESET(p23); + + +// pins for IR localization +Serial pc(USBTX, USBRX); // tx, rx +I2C i2c(p9, p10); // sda, scl +const int addr = 0xB0; // define the I2C Address of camera unsigned char txbuf[16] = {0}; unsigned char txlen = 0; @@ -66,7 +72,7 @@ double y_cent = -1; //speed of SCRIBE -float speed = 0.3; +float speed = 0.8; float t_factor = 0.15; @@ -137,8 +143,8 @@ // inputs blocked until shape finished if (mode != 13){ mode = (int) val[0]; + serial.printf("Mode 1-val: %i\r\n",mode); if(mode != 4 && mode !=6){ - serial.printf("Mode 1-val: %i\r\n",mode); osSignalClear(bluetooth_id,0x1); which_thread = 1; } @@ -168,6 +174,7 @@ if(rxlen == 5){ // in coordinates mode - accept negative coordinates if(mode != 4 && stroke == 0){ + serial.printf("*** Mode 1-val: %i\r\n",mode); mode = 9; } // in draw mode - convert coordinates @@ -186,7 +193,7 @@ //check if pen needs to be up or down path_p[counter] = (int) val[4]; - serial.printf("Pen mode: %i \r\n",path_p[counter]); + //serial.printf("Pen mode: %i \r\n",path_p[counter]); counter++; one_slot.release(); @@ -241,61 +248,15 @@ } } -void deadreckoning(){ - led2 = 1; - double sum; - int counter = 10; - int i; - - //calibration - sum = 0; -/* for (i = 0; i < 100; i++){ - sum += ain.read(); - } - double calibration = sum/100; - serial.printf("Calibration: %f\r\n",calibration);*/ - double threshold = 0.9; +void i2c_write2(int addr, char a, char b) +{ + char cmd[2]; - while(1){ - sum = 0; - step_f(); - for (i = 0; i < counter; i++){ - sum += ain.read(); - } - if(sum/counter > threshold){ - led3 = 1; - break; - } - serial.printf("Read Values: %f\r\n",sum/counter); - } - serial.printf("Turn left now \r\n"); - - //left by 90 degrees - int control; - double currentAngle = L.getAngle(); - double targetAngle = fmod(currentAngle+90,360); - while(fmod(abs(L.getAngle() - targetAngle),360)> 3){ - control = step_right(); - } - - while(1){ - sum = 0; - step_f(); - for (i = 0; i < counter; i++){ - sum += ain.read(); - } - if(sum/counter > threshold){ - led3 = 0; - break; - } - serial.printf("Read Values: %f\r\n",sum/counter); - } - serial.printf("Deadreckoning finished \r\n"); - L.reset(); + cmd[0] = a; + cmd[1] = b; + i2c.write(addr, cmd, 2); + wait(0.07); // delay 70ms } - - - void move_thread(void const *args){ @@ -336,9 +297,9 @@ while(1){ // check what mode is present serial.printf("Cylce \r\n"); - serial.printf("mode: %i \r\n",mode); Thread::signal_wait(0x1); + serial.printf("Mode Mover: %i \r\n",mode); if(mode == -1){ serial.printf("here ends"); @@ -499,11 +460,11 @@ //put pen down if (p_mode == 1){ serial.printf("-- Pen Down\r\n"); - L.servo(108); + L.servo(65); } //put pen up else if (p_mode == 0){ - L.servo(90); + L.servo(55); serial.printf("-- Pen Up\r\n"); } @@ -568,7 +529,7 @@ //L.servo(90); wait(1); serial.printf("Starting Dead-Reckoning\r\n"); - //deadreckoning(); + //ir_localization(); serial.printf("Starting the threads\r\n"); Thread bluetooth(bluetooth_thread);