![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
The servo version of SCRIBE
Dependencies: BLE_nRF8001 BNO055 HC_SR04_Ultrasonic_Library mbed-rtos mbed
Fork of SCRIBE_stepper by
Diff: main.cpp
- Revision:
- 14:82248fb06e53
- Parent:
- 13:d49cb8b52a1e
--- a/main.cpp Fri May 06 00:18:02 2016 +0000 +++ b/main.cpp Thu May 12 06:17:49 2016 +0000 @@ -25,6 +25,14 @@ DigitalInOut BLE_RDY(p21); DigitalInOut BLE_REQ(p22); DigitalInOut BLE_RESET(p23); + + +// pins for IR localization +I2C i2c(p9, p10); // sda, scl +const int addr = 0xB0; // define the I2C Address of camera +int ir_finish = 0; + +void back_to_zero(); unsigned char txbuf[16] = {0}; unsigned char txlen = 0; @@ -65,6 +73,8 @@ double y_pen = 0; double x_cent = 0; double y_cent = -1; +double x_taro = 0; +double y_taro = 0; //speed of SCRIBE float speed = 0.3; @@ -75,7 +85,7 @@ localization L; -osThreadId bluetooth_id, mover_id; +osThreadId bluetooth_id, mover_id,ir_id; void bluetooth_thread(void const *args){ bluetooth_id = Thread::gettid(); @@ -138,8 +148,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; } @@ -232,16 +242,219 @@ } } - -// 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 i2c_write2(int addr, char a, char b) +{ + char cmd[2]; + + cmd[0] = a; + cmd[1] = b; + i2c.write(addr, cmd, 2); + wait(0.07); // delay 70ms +} + +void clock_init() +{ + // set up ~20-25MHz clock on p21 + LPC_PWM1->TCR = (1 << 1); // Reset counter, disable PWM + LPC_SC->PCLKSEL0 &= ~(0x3 << 12); + LPC_SC->PCLKSEL0 |= (1 << 12); // Set peripheral clock divider to /1, i.e. system clock + LPC_PWM1->MR0 = 4; // Match Register 0 is shared period counter for all PWM1 + LPC_PWM1->MR6 = 2; // Pin 21 is PWM output 6, so Match Register 6 + LPC_PWM1->LER |= 1; // Start updating at next period start + LPC_PWM1->TCR = (1 << 0) || (1 << 3); // Enable counter and PWM +} + +void cam_init() +{ + // Init IR Camera sensor + i2c_write2(addr, 0x30, 0x01); + i2c_write2(addr, 0x30, 0x08); + i2c_write2(addr, 0x06, 0x90); + i2c_write2(addr, 0x08, 0xC0); + i2c_write2(addr, 0x1A, 0x40); + i2c_write2(addr, 0x33, 0x33); + wait(0.1); +} + +void ir_localization(void const *args) { + ir_id = Thread::gettid(); + char cmd[8]; + char buf[16]; + int Ix1,Iy1,Ix2,Iy2; + int Ix3,Iy3,Ix4,Iy4; + double angle1,angle2,angle3,angle4; + double angle_init, sum_x; + int s; + + // variables for localization + int m_count = 0; + double alpha1,alpha2,phi1,phi2,phiIMU1,phiIMU2,beta; + double x,y,z; + + // height is given + double h = 97; + + //clock_init(); + + // PC serial output + serial.printf("Initializing camera..."); + + cam_init(); + + serial.printf("complete\n"); + + float testAngle = L.getAngle(); + angle_init = L.getAngle(); + + serial.printf("Initial IMU-Angle is: %f\r\n",angle_init); + servo_slowleft(); + // read I2C stuff + while(m_count !=2){ + // IR Sensor read + cmd[0] = 0x36; + i2c.write(addr, cmd, 1); + i2c.read(addr, buf, 16); // read the 16-byte result + + Ix1 = buf[1]; + Iy1 = buf[2]; + s = buf[3]; + Ix1 += (s & 0x30) <<4; + Iy1 += (s & 0xC0) <<2; + + Ix2 = buf[4]; + Iy2 = buf[5]; + s = buf[6]; + Ix2 += (s & 0x30) <<4; + Iy2 += (s & 0xC0) <<2; + + Ix3 = buf[7]; + Iy3 = buf[8]; + s = buf[9]; + Ix3 += (s & 0x30) <<4; + Iy3 += (s & 0xC0) <<2; + + Ix4 = buf[10]; + Iy4 = buf[11]; + s = buf[12]; + Ix4 += (s & 0x30) <<4; + Iy4 += (s & 0xC0) <<2; + + angle1 = 45*((double)Ix1 / 1023 - 0.5); + angle2 = 45*((double)Ix2 / 1023 - 0.5); + angle3 = 45*(double)Ix3 / 1023; + angle4 = 45*(double)Ix4 / 1023; + + // print the coordinate data + serial.printf("Ix1: %4d, Iy1: %4d\n", Ix1, Iy1 ); + //serial.printf("Ix2: %4d, Iy2: %4d\n", Ix2, Iy2 ); + //serial.printf("Current IMU-Angle is: %f\r\n",L.getAngle()); + //print when more than one LED present + + if(Ix1 < 1023){ + //serial.printf("%d, %d: %f; %d, %d : %f\r\n", Ix1, Iy1, angle1, Ix2, Iy2, angle2); +/* if(angle1 <= -2){ + //serial.printf("Turn left! \r\n"); + } + else if (angle1 >= 2 && (abs(360 - L.getAngle()) > 90)){ + serial.printf("Turn right! \r\n"); + }*/ + if(abs(angle1) < 2) { + if(m_count == 0 && abs(360 - L.getAngle()) < 90){ + sum_x = 0; + // get ten pictures + for(int i = 0; i < 10; i++){ + cmd[0] = 0x36; + i2c.write(addr, cmd, 1); + i2c.read(addr, buf, 16); + + Ix1 = buf[1]; + Iy1 = buf[2]; + s = buf[3]; + Ix1 += (s & 0x30) <<4; + Iy1 += (s & 0xC0) <<2; + angle1 = 45*((double)Ix1 / 1023 - 0.5); + sum_x = sum_x + angle1; + } + serial.printf("*** Got average angle1: %f\r\n",sum_x/10); + phiIMU1 = L.getAngle(); + phi1 = 360 - (phiIMU1 + sum_x/10); + serial.printf("*** Got phi1: %f, got phi_IMU1: %f\r\n", phi1,phiIMU1); + m_count = 1; + } + else if (m_count == 1 && abs(360 - L.getAngle()) > 90){ + servo_stop(); + phiIMU2 = L.getAngle(); + + sum_x = 0; + // get ten pictures + for(int i = 0; i < 10; i++){ + cmd[0] = 0x36; + i2c.write(addr, cmd, 1); + i2c.read(addr, buf, 16); + + Ix1 = buf[1]; + Iy1 = buf[2]; + s = buf[3]; + Ix1 += (s & 0x30) <<4; + Iy1 += (s & 0xC0) <<2; + angle2 = 45*((double)Ix1 / 1023 - 0.5); + + sum_x = sum_x + angle2; + } + serial.printf("*** Got average angle2: %f\r\n",sum_x/10); + + phi2 = 360 - (phiIMU2 + sum_x/10); + serial.printf("*** Got phi2: %f, got phi_IMU2: %f\r\n", phi2,phiIMU2); + alpha1 = 90 - phi1; + alpha2 = 90 - alpha1; + beta = 180 - (phi2 - phi1) - alpha2; + z = h*sin(beta/180*3.1415)/sin((phi2 - phi1)/180*3.1415); + x = z*sin(phi1/180*3.1415); + y = z*sin(alpha1/180*3.1415); + + serial.printf("Angles are: phi1 = %f, phi2 = %f:\r\n",phi1, phi2); + serial.printf("Angles are: alpha1 = %f, alpha2 = %f:\r\n",alpha1, alpha2); + + //reset after print out and set coordinates + m_count = 2; + x_pen = rint(x/4.5); + y_pen = rint((h-y)/4.5); + + //correction for translation + x_pen = x_pen + 2; + y_pen = y_pen - 2; + x_cent = x_pen; + y_cent = y_pen - 1; + x_taro = x_pen; + y_taro = y_pen; + serial.printf("Current location: x = %f, y = %f \r\n",x,h-y); + serial.printf("Current coordinates Pen: x = %f, y = %f \r\n",x_pen,y_pen); + serial.printf("Current coordinates Center: x = %f, y = %f \r\n",x_cent,y_cent); + } + } + + } + wait(0.2); } + + //turn upside again + back_to_zero(); + ir_finish = 1; } +void back_to_zero(){ + float testAngle = L.getAngle(); + serial.printf("Current Angle BTZ: %f",L.getAngle()); + while(L.getAngle()> 1){ + if(L.getAngle() > 180){ + servo_slowright(); + } + else{ + servo_slowleft(); + } + } + servo_stop(); +} void move_thread(void const *args){ @@ -252,8 +465,6 @@ Timer t; //initally put pen down - servo_reset(); - servo_stop(); float currentAngle; float targetAngle; @@ -275,8 +486,6 @@ double x_tar,y_tar; double x_pen_pr, y_pen_pr, x_cent_pr, y_cent_pr, x_taro_pr, y_taro_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; @@ -340,7 +549,7 @@ else if(mode == 1){ serial.printf("Draw circle \r\n"); //call circle function - control = draw_circle(radius); + //control = draw_circle(radius); mode = -1; } @@ -393,7 +602,7 @@ else if (mode == 7){ if (first == 0){ serial.printf("Draw freely \r\n"); - draw_corr = 1; + draw_corr = 0.08; } first = 1; } @@ -410,8 +619,12 @@ if (start == size){ start = 0; } - x_tar = (double) path_x[start]*draw_corr; - y_tar = (double) path_y[start]*draw_corr; + x_tar = (double) path_x[start]; + y_tar = (double) path_y[start]; + serial.printf("Got data: x = %f, y = %f \r\n",x_tar,y_tar); + x_tar = x_tar*draw_corr; + y_tar = y_tar*draw_corr; + serial.printf("Scaled data: x = %f, y = %f \r\n",x_tar,y_tar); p_mode = path_p[start]; start++; if(start == counter){ @@ -486,11 +699,18 @@ //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 - serial.printf("Distance to be taken: %f\r\n",distance); - + //forward SCRIBE til target int counter_m = (int) rint(distance); + // do adjustment when going down + if (L.getAngle() > 330 || L.getAngle() < 30){ + counter_m = counter_m + 2; + } + if (L.getAngle() > 130 || L.getAngle() < 230){ + counter_m = counter_m - 2; + } + serial.printf("*** Distance to be taken: %d\r\n",counter_m); + // find position of wheels for (int i = 0; i < 100; i++){ sum_r += ain1.read(); @@ -505,7 +725,7 @@ else{ on_r = 1; } - if(value_l < 0.93){ + if(value_l < 0.925){ on_l = 0; } else{ @@ -521,32 +741,33 @@ while (counter_m > 0){ sum_r = 0; sum_l = 0; - for (int i = 0; i < 100; i++){ + for (int i = 0; i < 5; i++){ sum_r += ain1.read(); sum_l += ain2.read(); } - value_r = sum_r/100; - value_l = sum_l/100; - //printf("Value: %f\r\n",value); - if(value_r < 0.93 && on_r == 1){ + value_r = sum_r/5; + value_l = sum_l/5; + printf("Left Value: %f\r\n",value_l); + printf("Right Value: %f\r\n",value_r); + if(value_r < 0.84 && on_r == 1){ //printf("Value right: %f\r\n",value_r); on_r = 0; } - else if (value_r > 0.95 && on_r == 0){ + else if (value_r > 0.88 && on_r == 0){ //printf("Value right: %f\r\n",value_r); on_r = 1; counter_r--; - //printf("*** Right Counter is: %d\r\n",counter_r); + printf("*** Right Counter is: %d\r\n",counter_r); } - if(value_l < 0.93 && on_l == 1){ + if(value_l < 0.9 && on_l == 1){ //printf("Value left: %f\r\n",value_l); on_l = 0; } - else if (value_l > 0.95 && on_l == 0){ + else if (value_l > 0.92 && on_l == 0){ //printf("Value left: %f\r\n",value_l); on_l = 1; counter_l--; - //printf("*** Left Counter is: %d\r\n",counter_l); + printf("*** Left Counter is: %d\r\n",counter_l); } if(counter_l >= counter_r){ @@ -556,17 +777,20 @@ counter_m = counter_l; } //deviation left from path - if(L.getAngle() - angle_init > 2 || (angle_init > 358 && L.getAngle() < 5 && (L.getAngle() + 360 - angle_init) > 2)){ - servo_slowleft(); - serial.printf("Angle Difference: %f\r\n",L.getAngle() - angle_init); - while(fmod(abs(L.getAngle() - angle_init),360)> 1); - servo_f(); - } - //deviation right from path - if(angle_init - L.getAngle() > 2 ){ - servo_slowright(); - while(fmod(abs(L.getAngle() - angle_init),360)> 1); - servo_f(); + if(angle_init > 270 || angle_init < 90){ + if((L.getAngle() - angle_init > 2 && L.getAngle() - angle_init < 10) || (angle_init > 358 && L.getAngle() < 5 && (L.getAngle() + 360 - angle_init) > 2)){ + servo_slowleft(); + serial.printf("Left: Angle Difference: %f\r\n",L.getAngle() - angle_init); + serial.printf("Left: Init: %f, Current: %f\r\n",angle_init,L.getAngle()); + while(fmod(abs(L.getAngle() - angle_init),360)> 1); + servo_f(); + } + //deviation right from path + if(angle_init - L.getAngle() > 2 || (L.getAngle() > 358 && angle_init < 5 && (angle_init + 360 - L.getAngle()) > 2)){ + servo_slowright(); + while(fmod(abs(L.getAngle() - angle_init),360)> 1); + servo_f(); + } } //printf("*** Distance to take is: %d\r\n",counter_m); } @@ -605,10 +829,19 @@ int main() { L.reset(); - //L.servo(90); - wait(1); - serial.printf("Starting Dead-Reckoning\r\n"); - //deadreckoning(); + servo_reset(); + servo_stop(); + serial.printf("Starting Calibration\r\n"); + back_to_zero(); + + serial.printf("Starting IR-Localization\r\n"); + //ir_localization(); + Thread ir(ir_localization); + ir_id = ir.gettid(); + + while(ir_finish == 0){ + Thread::wait(10); + } serial.printf("Starting the threads\r\n"); Thread bluetooth(bluetooth_thread);