SCRIBE
scribe robot working with stepper motors
Dependencies: BLE_nRF8001 BNO055 HC_SR04_Ultrasonic_Library localization mbed-rtos mbed
Revision 6:ca112c3083bb, committed 2016-05-12
- Comitter:
- manz
- Date:
- Thu May 12 06:12:45 2016 +0000
- Parent:
- 5:2c196f871096
- Commit message:
- latest version;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| stepper.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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);
--- a/stepper.cpp Wed May 04 23:51:57 2016 +0000
+++ b/stepper.cpp Thu May 12 06:12:45 2016 +0000
@@ -11,16 +11,17 @@
DigitalOut brown_r(p19);
DigitalOut black_r(p20);
-int wait_t = 60;
+int wait_t = 20;
int case_stepf = 0;
+int case_stepb = 0;
int case_right = 0;
int case_left = 0;
int case_rone = 0;
//StepperMotorUni motor( p17, p18, p19, p20);
-int step_f(){
- switch(case_stepf){
+int step_b(){
+ switch(case_stepb){
case 0:
//step 0101
black_l = 0;
@@ -33,7 +34,7 @@
yellow_r = 1;
wait_ms(wait_t);
- case_stepf=1;
+ case_stepb=1;
break;
case 1:
@@ -48,7 +49,7 @@
yellow_r = 0;
wait_ms(wait_t);
- case_stepf=2;
+ case_stepb=2;
break;
case 2:
@@ -63,7 +64,7 @@
yellow_r = 0;
wait_ms(wait_t);
- case_stepf = 3;
+ case_stepb = 3;
break;
case 3:
@@ -78,62 +79,75 @@
yellow_r = 1;
wait_ms(wait_t);
- case_stepf = 0;
+ case_stepb = 0;
break;
}
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);
-
+int step_f(){
+ switch(case_stepf){
+ case 0:
+ //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);
+ case_stepf = 1;
+ break;
+
+ case 1:
+ //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);
+ case_stepf = 2;
+ break;
+
+ case 2:
+ //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);
+ case_stepf = 3;
+ break;
+
+ case 3:
+ //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);
+ case_stepf = 0;
+ break;
+ }
return 1;
}