scribe robot working with stepper motors
Dependencies: BLE_nRF8001 BNO055 HC_SR04_Ultrasonic_Library localization mbed-rtos mbed
main.cpp
- Committer:
- manz
- Date:
- 2016-04-25
- Revision:
- 4:1da4d4050306
- Parent:
- 2:63aef644e6d2
- Child:
- 5:2c196f871096
File content as of revision 4:1da4d4050306:
// main of SCRIBE stepper // Import libraries #include "Arduino.h" #include "BLEPeripheral.h" #include "mbed.h" #include "rtos.h" #include "localization.h" #include "stepper.h" Serial serial(USBTX, USBRX); SPI spi(p11, p12, p13); DigitalInOut BLE_RDY(p21); DigitalInOut BLE_REQ(p22); DigitalInOut BLE_RESET(p23); unsigned char txbuf[16] = {0}; unsigned char txlen = 0; // create peripheral BLEPeripheral blePeripheral = BLEPeripheral(&BLE_REQ, &BLE_RDY, &BLE_RESET); // create service w/ uuid BLEService uartService = BLEService("713d0000503e4c75ba943148f18d941e"); // create characteristics BLECharacteristic txCharacteristic = BLECharacteristic("713d0002503e4c75ba943148f18d941e", BLENotify, 20); BLECharacteristic rxCharacteristic = BLECharacteristic("713d0003503e4c75ba943148f18d941e", BLEWriteWithoutResponse, 20); unsigned int interval = 0; unsigned char count_on = 0; int which_thread = 0; // array to save the values of the user const int size = 100; int path_x [size]; int path_y [size]; int path_p [size]; int counter = 0; int start = 0; const int shape_size = 10; int shape_x [shape_size]; int shape_y [shape_size]; int mode = -1; //default mode is idle int pmode = 1; //default pen is down //coordinates of the pen and the centre of SCRIBE at beginning double x_pen = 0; double y_pen = 0; double x_cent = 0; double y_cent = -1; //speed of SCRIBE float speed = 0.3; float t_factor = 0.15; Semaphore one_slot(1); localization L; osThreadId bluetooth_id, mover_id; void bluetooth_thread(void const *args){ bluetooth_id = Thread::gettid(); serial.printf("Serial begin!\r\n"); FILE *fp = fopen("/local/trial.m", "w"); if (fp == NULL) { serial.printf("ERROR: File open!\r\n"); } int x,y,x_dir,y_dir; /*----- BLE Utility ---------------------------------------------*/ // set advertised local name and service UUID blePeripheral.setLocalName("BLE Shield"); blePeripheral.setAdvertisedServiceUuid(uartService.uuid()); // add service and characteristic blePeripheral.addAttribute(uartService); blePeripheral.addAttribute(rxCharacteristic); blePeripheral.addAttribute(txCharacteristic); // begin initialization blePeripheral.begin(); /*---------------------------------------------------------------*/ serial.printf("BLE UART Peripheral begin!\r\n"); while(1) { BLECentral central = blePeripheral.central(); if (central) { // central connected to peripheral serial.printf("Connected to central\r\n"); while (central.connected()) { Thread::signal_wait(0x1); // central still connected to peripheral if (rxCharacteristic.written()) { unsigned char rxlen = rxCharacteristic.valueLength(); const unsigned char *val = rxCharacteristic.value(); /*serial.printf("didCharacteristicWritten, Length: %d\r\n", rxlen); unsigned char i = 0; while(i<rxlen) { serial.printf("%d, ",val[i++]); } serial.printf("\r\n");*/ //determine mode of signal if(rxlen == 1){ // inputs blocked until shape finished if (mode != 13){ mode = (int) val[0]; if(mode != 4 && mode !=6){ which_thread = 1; osSignalClear(bluetooth_id,0x1); } } } if (mode == 6){ serial.printf("Storing drawn coordinates\r\n"); serial.printf("x_coord = ["); for (int i = 0; i < size; i++) { serial.printf("%i ", path_x[i]); } serial.printf("];\n"); serial.printf("y_coord = ["); for (int i = 0; i < size; i++) { serial.printf("%i ", path_y[i]); } serial.printf("];\n"); } // stroke of pen finished if (mode == 7){ serial.printf("Other threads can proceed \r\n"); osSignalClear(bluetooth_id,0x1); which_thread = 1; } if(rxlen == 5){ // in coordinates mode - accept negative coordinates if(mode == 6){ mode = 9; } // in draw mode - convert coordinates x = (int) val[0]*256 + val[1]; y = (int) val[2]*256 + val[3]; // putting values into array //serial.printf("try to put coordinates \r\n"); one_slot.wait(); if (counter == size){ serial.printf("Overwriting values \r\n"); counter = 0; } path_x[counter] = x; path_y[counter] = y; //check if pen needs to be up or down path_p[counter] = pmode; counter++; one_slot.release(); if(mode == 9){ which_thread = 1; osSignalClear(bluetooth_id,0x1); } //serial.printf("put coordinates \r\n"); } } if(serial.readable()) { if(!count_on) { count_on = 1; } interval = 0; txbuf[txlen] = serial.getc(); txlen++; } if(count_on) // Count the interval after receiving a new char from terminate { interval++; } if(interval == 10) // If there is no char available last the interval, send the received chars to central. { interval = 0; count_on = 0; //serial.printf("Received from terminal: %d bytes\r\n", txlen); txCharacteristic.setValue((const unsigned char *)txbuf, txlen); txlen = 0; } } // central disconnected serial.printf("Disconnected from central\r\n"); } } } // incomplete int draw_circle(double radius){ float currAngle = L.getAngle(); if(radius >= 5){ int outer = (int) radius*0.6; int inner = (int) radius*0.2; } } void move_thread(void const *args){ int angle; double length = 40; double width = 40; int radius = 5; Timer t; t.start(); L.reset(); //initally put pen down //L.servo(0); float currentAngle; float targetAngle; float startAngle; float diffAngle; int steps; int control; int shape_count = 0; int p_mode; int first = 0; double draw_corr = 5; double dot,a2,b2,c2; double x_tar,y_tar; double x_pen_pr, y_pen_pr, x_cent_pr, y_cent_pr; double cosg,gamma,distance,wait_t; double x_taro = 0; double y_taro = 0; serial.printf("Started move thread\r\n"); int newval = 0; while(1){ // check what mode is present serial.printf("Cylce \r\n"); Thread::signal_wait(0x1); if(mode == -1){ serial.printf("here ends"); osSignalClear(mover_id,0x1); which_thread = 0; } //rectangle/square else if(mode == 0){ serial.printf("Draw rectangle \r\n"); //save old values and set initial x_pen_pr = x_pen; y_pen_pr = y_pen; x_cent_pr = x_cent; y_cent_pr = y_cent; x_pen = 0; y_pen = 0; x_cent = 0; y_cent = -1; x_taro = 0; y_taro = 0; //set values shape_x[3] = 0; shape_y[3] = length; shape_x[2] = width; shape_y[2] = length; shape_x[1] = width; shape_y[1] = 0; shape_x[0] = 0; shape_y[0] = 0; shape_count = 4; mode = 13; } else if(mode == 1){ serial.printf("Draw circle \r\n"); //call circle function control = draw_circle(radius); mode = -1; } else if(mode == 2){ serial.printf("Draw triangle \r\n"); //save old values and set initial x_pen_pr = x_pen; y_pen_pr = y_pen; x_cent_pr = x_cent; y_cent_pr = y_cent; x_pen = 0; y_pen = 0; x_cent = 0; y_cent = -1; x_taro = 0; y_taro = 0; //set values shape_x[2] = 0; shape_y[2] = length; shape_x[1] = width; shape_y[1] = 0; shape_x[0] = 0; shape_y[0] = 0; shape_count = 3; mode = 13; } else if(mode == 3){ serial.printf("Reset \r\n"); //set initial x_pen = 0; y_pen = 0; x_cent = 0; y_cent = -1; mode = -1; } else if (mode == 7){ if (first == 0){ serial.printf("Draw freely \r\n"); draw_corr = 1; } first = 1; } else if (mode == 9){ serial.printf("Draw coordinates \r\n"); draw_corr = 5; mode = -1; } // next coordinate is free drawing or coordinates if(shape_count == 0){ one_slot.wait(); if(counter != start){ if (start == size){ start = 0; } x_tar = (double) path_x[start]*draw_corr; y_tar = (double) path_y[start]*draw_corr; p_mode = path_p[start]; start++; if(start == counter) mode = -1; newval = 1; } one_slot.release(); } // next coordinate is shape else{ shape_count = shape_count - 1; x_tar = (double) shape_x[shape_count]; y_tar = (double) shape_y[shape_count]; p_mode = 1; newval = 1; //last move -> unblock input if(shape_count == 0){ mode = -1; } } if(newval == 1){ serial.printf("x-coord: %f, y-coord: %f\r\n",x_tar,y_tar); newval = 0; //compute angle and turn direction a2 = (x_pen - x_cent)*(x_pen - x_cent) + (y_pen - y_cent)*(y_pen - y_cent); b2 = (x_tar - x_pen)*(x_tar - x_pen) + (y_tar - y_pen)*(y_tar - y_pen); c2 = (x_tar - x_cent)*(x_tar - x_cent) + (y_tar - y_cent)*(y_tar - y_cent); cosg = (a2 + b2 - c2)/(2*sqrt(a2*b2)); gamma = acos(cosg)/3.14159*180; dot = (x_tar - x_cent)*(y_pen - y_cent) - (y_tar - y_cent)*(x_pen - x_cent); //serial.printf("Angle: %f \r\n",gamma); angle = ceil(180 - gamma); serial.printf("Turning angle: %i \r\n",angle); //put pen down if (p_mode == 1){ //serial.printf("Pen down \r\n"); L.servo(100); } //put pen up else if (p_mode == 0){ //serial.printf("Pen up \r\n"); L.servo(90); } currentAngle = L.getAngle(); if(dot > 0){ //serial.printf("Turn right \r\n"); targetAngle = fmod(currentAngle+angle,360); while(fmod(abs(L.getAngle() - targetAngle),360)> 3){ control = step_right(); //serial.printf("Turning angle: %f \r\n",fmod(abs(L.getAngle() - targetAngle),360)); } //serial.printf("Reached target \r\n"); } else if(dot < 0){ //serial.printf("Turn left \r\n"); targetAngle = fmod(currentAngle-angle,360); while(fmod(abs(L.getAngle() - targetAngle),360)> 3){ control = step_left(); //serial.printf("Turning angle: %f \r\n",fmod(abs(L.getAngle() - targetAngle),360)); } } //compute length of path til target distance = sqrt((x_tar - x_pen)*(x_tar - x_pen) + (y_tar - y_pen)*(y_tar - y_pen)); //forward SCRIBE til target steps = (int) distance; for(int i = 0; i< steps; i++){ control = step_f(); } //serial.printf("Reached destination \r\n"); //update pen and center when at target x_pen = x_tar; y_pen = y_tar; serial.printf("Update Pen: %f, %f \r\n",x_pen,y_pen); x_cent = x_taro; y_cent = y_taro; serial.printf("Update Center: %f, %f \r\n",x_cent,y_cent); x_taro = x_tar; y_taro = y_tar; } } } int main() { serial.printf("Starting the threads"); Thread bluetooth(bluetooth_thread); Thread move(move_thread); mover_id = move.gettid(); bluetooth_id = bluetooth.gettid(); while(1){ if(which_thread == 0) bluetooth.signal_set(0x1); else move.signal_set(0x1); } }