Revision 14:82248fb06e53, committed 2016-05-12

Comitter:

manz

Date:

Thu May 12 06:17:49 2016 +0000

Parent:

13:d49cb8b52a1e

Commit message:

latest version;

Changed in this revision

--- 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);

--- a/servo.cpp	Fri May 06 00:18:02 2016 +0000
+++ b/servo.cpp	Thu May 12 06:17:49 2016 +0000
@@ -9,8 +9,10 @@
 }
 
 void servo_f(){
-    pin25.pulsewidth_us(1300);
-    pin26.pulsewidth_us(1700);
+    //pin25.pulsewidth_us(1300);
+    //pin26.pulsewidth_us(1700);
+    pin25.pulsewidth_us(1350);
+    pin26.pulsewidth_us(1650);
 }
 
 void servo_b(){
@@ -30,7 +32,7 @@
 
 void servo_stop(){
     pin25.pulsewidth_us(1500);
-    pin26.pulsewidth_us(1510);                                      
+    pin26.pulsewidth_us(1515);                                      
 }
 
 void servo_slowright(){