SCRIBE
scribe robot working with stepper motors
Dependencies: BLE_nRF8001 BNO055 HC_SR04_Ultrasonic_Library localization mbed-rtos mbed
Revision 0:83acd45a2405, committed 2016-04-21
- Comitter:
- manz
- Date:
- Thu Apr 21 22:01:19 2016 +0000
- Child:
- 1:81fa6a4bb47f
- Commit message:
- stepper version of SCRIBE
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BLE_nRF8001.lib Thu Apr 21 22:01:19 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/teams/RedBearLab/code/BLE_nRF8001/#e9a412d69201
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BNO055.lib Thu Apr 21 22:01:19 2016 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/StressedDave/code/BNO055/#1f722ffec323
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HC_SR04_Ultrasonic_Library.lib Thu Apr 21 22:01:19 2016 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/ejteb/code/HC_SR04_Ultrasonic_Library/#e0f9c9fb4cf3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/localization.lib Thu Apr 21 22:01:19 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/SCRIBE/code/localization/#ee872bf49be6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Apr 21 22:01:19 2016 +0000
@@ -0,0 +1,302 @@
+// main of SCRIBE stepper
+
+// Import libraries
+#include "Arduino.h"
+#include "BLEPeripheral.h"
+#include "mbed.h"
+#include "stepper.h"
+#include "rtos.h"
+#include "localization.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;
+
+// array to save the values of the user
+const int size = 40;
+int path_x [size];
+int path_y [size];
+int counter = 0;
+int start = 0;
+
+//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);
+
+
+void bluetooth_thread(void const *args){
+ serial.printf("Serial begin!\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();
+ /*---------------------------------------------------------------*/
+
+ //return value for move functions
+ int ret;
+
+ 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())
+ {
+ // 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");
+ x = (int) val[0];
+ y = (int) val[1];
+ x_dir = (int) val[2];
+ y_dir = (int) val[3];
+
+ // see if values are negative
+ if(x_dir == 1){
+ x = -1*x;
+ }
+ if(y_dir == 1){
+ y = -1*y;
+ }
+ // go forward
+ if(x == 0 && y == 4){
+ serial.printf("Go forward \r\n");
+ }
+ // go backward
+ else if(x == 0 && y == -4){
+ serial.printf("Go backward \r\n");
+ }
+ // go left
+ else if(x == 4 && y == 0 && x_dir == 0 && y_dir == 1){
+ serial.printf("Go left \r\n");
+ }
+ // go right
+ else if(x == 4 && y == 0){
+ serial.printf("Go right \r\n");
+ }
+ else {
+ serial.printf("Coordinates \r\n");
+ }
+
+ // putting values into array
+ serial.printf("try to put coordinates \r\n");
+ one_slot.wait();
+ if (counter == size){
+ counter = 0;
+ }
+ path_x[counter] = x;
+ path_y[counter] = y;
+ counter++;
+ one_slot.release();
+ serial.printf("put coordinates \r\n");
+
+ Thread::wait(100);
+ }
+
+ 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");
+ }
+ }
+}
+
+
+void move_thread(void const *args){
+ int angle;
+
+ Timer t;
+ t.start();
+
+ localization L;
+ L.reset();
+ //initally put pen down
+ //L.servo(0);
+
+ float currentAngle;
+ float targetAngle;
+ float startAngle;
+ float diffAngle;
+
+ int control;
+
+ double dot,a2,b2,c2;
+ double x_tar,y_tar;
+ 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){
+ one_slot.wait();
+ if(counter != start){
+ if (start == size){
+ start = 0;
+ }
+ x_tar = path_x[start];
+ y_tar = path_y[start];
+ start++;
+ newval = 1;
+ }
+ one_slot.release();
+
+ 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 up
+ //L.servo(30);
+
+ 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",abs(L.getAngle() - targetAngle));
+ }
+ //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",abs(L.getAngle() - targetAngle));
+ }
+ }
+ else{
+ }
+ //put pen down again
+ //L.servo(0);
+
+ startAngle = L.getAngle();
+ serial.printf("Current angle: %f \r\n",startAngle);
+ //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
+ wait_t = distance/speed*t_factor;
+ t.reset();
+ while(t.read() < wait_t){
+ 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;
+
+ }
+ Thread::wait(100);
+ }
+}
+
+int main()
+{
+ serial.printf("Starting the threads");
+
+ Thread bluetooth(bluetooth_thread);
+ Thread move(move_thread);
+
+ Thread::wait(osWaitForever);
+
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Thu Apr 21 22:01:19 2016 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/mbed_official/code/mbed-rtos/#bdd541595fc5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Apr 21 22:01:19 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/082adc85693f \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/stepper.cpp Thu Apr 21 22:01:19 2016 +0000
@@ -0,0 +1,252 @@
+#include "mbed.h";
+
+DigitalOut yellow_l(p9);
+DigitalOut orange_l(p14);
+DigitalOut brown_l(p15);
+DigitalOut black_l(p16);
+
+DigitalOut yellow_r(p17);
+DigitalOut orange_r(p18);
+DigitalOut brown_r(p19);
+DigitalOut black_r(p20);
+
+int wait_t = 60;
+int case_right = 0;
+int case_left = 0;
+
+//StepperMotorUni motor( p17, p18, p19, p20);
+int step_f(){
+ //step 0101
+ black_l = 0;
+ orange_l = 0;
+ brown_l = 1;
+ yellow_l = 1;
+ black_r = 1;
+ orange_r = 0;
+ brown_r = 0;
+ yellow_r = 1;
+
+ wait_ms(wait_t);
+
+ //step 1100
+ black_l = 0;
+ orange_l = 1;
+ brown_l = 1;
+ yellow_l = 0;
+ black_r = 1;
+ orange_r = 1;
+ brown_r = 0;
+ yellow_r = 0;
+
+ wait_ms(wait_t);
+
+ //step 1010
+ black_l = 1;
+ orange_l = 1;
+ brown_l = 0;
+ yellow_l = 0;
+ black_r = 0;
+ orange_r = 1;
+ brown_r = 1;
+ yellow_r = 0;
+
+ wait_ms(wait_t);
+
+ //step 0011
+ black_l = 1;
+ orange_l = 0;
+ brown_l = 0;
+ yellow_l = 1;
+ black_r = 0;
+ orange_r = 0;
+ brown_r = 1;
+ yellow_r = 1;
+
+ wait_ms(wait_t);
+
+ return 1;
+}
+
+
+int step_b(){
+ //step 0101
+ black_r = 0;
+ orange_r = 0;
+ brown_r = 1;
+ yellow_r = 1;
+ black_l = 1;
+ orange_l = 0;
+ brown_l = 0;
+ yellow_l = 1;
+
+ wait_ms(wait_t);
+
+ //step 1100
+ black_r = 0;
+ orange_r = 1;
+ brown_r = 1;
+ yellow_r = 0;
+ black_l = 1;
+ orange_l = 1;
+ brown_l = 0;
+ yellow_l = 0;
+
+ wait_ms(wait_t);
+
+ //step 1010
+ black_r = 1;
+ orange_r = 1;
+ brown_r = 0;
+ yellow_r = 0;
+ black_l = 0;
+ orange_l = 1;
+ brown_l = 1;
+ yellow_l = 0;
+
+ wait_ms(wait_t);
+
+ //step 0011
+ black_r = 1;
+ orange_r = 0;
+ brown_r = 0;
+ yellow_r = 1;
+ black_l = 0;
+ orange_l = 0;
+ brown_l = 1;
+ yellow_l = 1;
+
+ wait_ms(wait_t);
+
+ return 1;
+}
+
+int step_right(){
+ switch(case_right){
+ case 0:
+ //step 0101
+ black_r = 0;
+ orange_r = 0;
+ brown_r = 1;
+ yellow_r = 1;
+ black_l = 0;
+ orange_l = 0;
+ brown_l = 1;
+ yellow_l = 1;
+
+ wait_ms(wait_t);
+ case_right = 1;
+ break;
+
+ case 1:
+ //step 1100
+ black_r = 0;
+ orange_r = 1;
+ brown_r = 1;
+ yellow_r = 0;
+ black_l = 0;
+ orange_l = 1;
+ brown_l = 1;
+ yellow_l = 0;
+
+ wait_ms(wait_t);
+ case_right = 2;
+ break;
+
+
+ case 2:
+ //step 1010
+ black_r = 1;
+ orange_r = 1;
+ brown_r = 0;
+ yellow_r = 0;
+ black_l = 1;
+ orange_l = 1;
+ brown_l = 0;
+ yellow_l = 0;
+
+ wait_ms(wait_t);
+ case_right = 3;
+ break;
+
+
+ case 3:
+ //step 0011
+ black_r = 1;
+ orange_r = 0;
+ brown_r = 0;
+ yellow_r = 1;
+ black_l = 1;
+ orange_l = 0;
+ brown_l = 0;
+ yellow_l = 1;
+
+ wait_ms(wait_t);
+ case_right = 0;
+ break;
+ }
+ return 1;
+}
+
+
+int step_left(){
+ switch(case_left){
+ case 0:
+ //step 0011
+ black_l = 1;
+ orange_l = 0;
+ brown_l = 0;
+ yellow_l = 1;
+ black_r = 1;
+ orange_r = 0;
+ brown_r = 0;
+ yellow_r = 1;
+
+ wait_ms(wait_t);
+ case_left = 1;
+ break;
+
+ case 1:
+ black_l = 1;
+ orange_l = 1;
+ brown_l = 0;
+ yellow_l = 0;
+ black_r = 1;
+ orange_r = 1;
+ brown_r = 0;
+ yellow_r = 0;
+
+ wait_ms(wait_t);
+ case_left = 2;
+ break;
+
+ case 2:
+ black_l = 0;
+ orange_l = 1;
+ brown_l = 1;
+ yellow_l = 0;
+ black_r = 0;
+ orange_r = 1;
+ brown_r = 1;
+ yellow_r = 0;
+
+ wait_ms(wait_t);
+ case_left = 3;
+ break;
+
+ case 3:
+ black_l = 0;
+ orange_l = 0;
+ brown_l = 1;
+ yellow_l = 1;
+ black_r = 0;
+ orange_r = 0;
+ brown_r = 1;
+ yellow_r = 1;
+
+ wait_ms(wait_t);
+
+ case_left = 0;
+ break;
+ }
+ return 1;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/stepper.h Thu Apr 21 22:01:19 2016 +0000 @@ -0,0 +1,7 @@ +int step_f(); + +int step_b(); + +int step_left(); + +int step_right(); \ No newline at end of file