ros_machine_interface.h

00001 #ifndef ROS_MACHINE_INTERFACE_H
00002 #define ROS_MACHINE_INTERFACE_H
00003 //THIS FILE IS INCOMPLETE///
00004 
00005 
00006 /**This file contains the implementations for the controls through a python interface
00007 //The brain file uses ';' to separate lines
00008 Read IMU [acceleration and gyros]
00009 Read Gyro
00010 Read Angle
00011 Read Acceleration
00012 Read IR sensors
00013 Read Optical flow sensors
00014 Read current sensor
00015 Read voltage sensor
00016 
00017 Drive to [x,y] (x,y)
00018 Drive fwd      (speed, time)
00019 Run R motor    (speed,time)
00020 Run L motor     (speed,time)
00021 Set Direction   (direction)
00022 Enable IMU control (bool)   //if this is enabled, IMU will keep robot straight
00023 */
00024 #include "keybindings.h"
00025 
00026 
00027 int ros_get_cmd(); //this reads from the bt connection
00028 int q_pow10(int n);     //this calculates 10^n quickly (only up to 10^6)
00029 void print_all();
00030 int ros_get_cmd(char s[16], int n);
00031 //char C;
00032 //char buffer[10];
00033 int IR_mm(float x);
00034 
00035 bool ros_shell_connection(char c)
00036 {
00037     char S[16];
00038     float dum;
00039     float param[5];
00040     int arg_in[10];
00041     char run_time;
00042     int m_offset = 100; //use this so that the MATLAB can send motor values between 0 and 200 instead of -100 and 100
00043     //bt.printf("&%f;",t.read());
00044 
00045     switch (c)
00046     {
00047         case CONNECTION:   //Return query if connection is ready
00048             bt.lock();
00049             bt.printf("\nEBOTDEMO\n");
00050             bt.unlock();
00051             reckon.reset();
00052             break;
00053         case ALL_SENSOR:    //returns ALL sensor data //while robot is moving, send things every 50milliseconds
00054             print_all();
00055             break;
00056         case CURRENT_SENSE:
00057             break; 
00058         case IMU_READ:
00059             send_flag = 1;
00060             //bt.lock();
00061             //bt.printf("i;%3.3f;%3.3f;%3.3f;%3.3f;%3.3f;%3.3f;",imu_data.acc[0],imu_data.acc[1],imu_data.acc[2],imu_data.ypr[0],imu_data.ypr[1],imu_data.ypr[2]);
00062             //bt.unlock();
00063             break;
00064         case IMU_YAW:
00065             dum = imu_data.ypr[0]*180/PI;
00066             bt.lock();
00067             bt.printf("y;%3.3f;",dum);
00068             bt.unlock();
00069             break;
00070         //optical flow
00071         case OPT_READ:
00072             //return optical flow (pixels/second for left and right)
00073             //TODO: include second optical flow sensor
00074             //bt.printf("O;%3.3f;%3.3f;",opt_flow.dx_px,opt_flow.dy_px);
00075             bt.lock();
00076             bt.printf("%c;%d;%d;%d;%d;",OPT_READ,reckon.flow_dx_L,reckon.flow_dy_L,reckon.flow_dx_R,reckon.flow_dy_R);
00077             bt.unlock();
00078             reckon.reset();
00079             break;
00080         case IR_READ:
00081             //return 5 IR sensors clockwise (Front, right_front, right_rear, left_rear, left_front)
00082             //ask Wen to fill out this function
00083             extern AnalogIn irL;
00084             extern AnalogIn irC;
00085             extern AnalogIn irR;
00086             extern DigitalOut irBack;
00087             if(irBack){
00088                 irBack = 0; //read from IR
00089                 Thread::wait(50);
00090             }
00091             param[0] = irL.read();
00092             Thread::wait(10);
00093             param[1] = irC.read();
00094             Thread::wait(10);
00095             param[2] = irR.read();
00096             param[3] = 0;
00097             param[4]=0;
00098             //irBack = 1; //read back IR
00099             /*Thread::wait(50);
00100             param[3] = irL.read();
00101             Thread::wait(10);
00102             param[4] = irR.read();
00103             irBack = 0;*/
00104             //bt.printf("%f;%f;%f;%f;%f;",param[0],param[1],param[2],param[3],param[4]);
00105             for(int i = 0; i<5; i++)
00106                 arg_in[i]=IR_mm(param[i]);
00107             bt.lock();
00108             bt.printf("%d;%d;%d;%d;%d;",arg_in[0],arg_in[1],arg_in[2],arg_in[3],arg_in[4]);
00109             bt.unlock();
00110             //Thread::wait(40);
00111             break;
00112         /*case IR_1:
00113             if(irBack){
00114                 irBack = 0;
00115                 Thread::wait(50);
00116             }
00117             param[0] = irL.read();
00118             bt.printf("%c;%f;",IR_1,param[0]);
00119             break;
00120         case IR_2:
00121             if(irBack){
00122                 irBack = 0;
00123                 Thread::wait(50);
00124             }
00125             param[0] = irC.read();
00126             bt.printf("%c;%f;",IR_2,param[0]);
00127         break;
00128         case IR_3:
00129             if(irBack){
00130                 irBack = 0;
00131                 Thread::wait(50);
00132             }
00133             param[0] = irR.read();
00134             bt.printf("%c;%f;",IR_3,param[0]);
00135         break;
00136         case IR_4:
00137             if(!irBack){
00138                 irBack = 1;
00139                 Thread::wait(50);
00140             }
00141             param[0] = irR.read();
00142             bt.printf("%c;%f;",IR_4,param[0]);
00143             break;
00144         case IR_5:
00145             if(!irBack){
00146                 irBack = 1;
00147                 Thread::wait(50);
00148             }
00149             param[0] = irL.read();
00150             bt.printf("%c;%f;",IR_5,param[0]);
00151         break;*/
00152         case LOCATION:
00153             //return x,y,z,theta (maybe not z)
00154             break;
00155             
00156         case MOTOR:   //command: 'w[L_speed],[R_speed]      //I should write an actual function to receive data from serial
00157             //Another argument should be included: time to run motors.
00158             arg_in[0] = ros_get_cmd();
00159             arg_in[1] = ros_get_cmd();
00160             arg_in[2] = ros_get_cmd();
00161             if(arg_in[0] == arg_in[1] && arg_in[0] > 100){
00162                 if(IR_mm(irC.read()) < 260 && IR_mm(irC.read()) > 0) //if there is an obstacle, don't drive fwd
00163                 {   
00164                     bt.lock();
00165                     bt.printf("&;OBSTACLE;");
00166                     bt.unlock();
00167                     break;
00168                 }
00169                 fwd_flag = 1;
00170             }
00171             moveMotors(arg_in[0]-m_offset,arg_in[1]-m_offset,arg_in[2]);
00172             bt.lock();
00173             bt.printf("L=%d;R=%d;t=%d",arg_in[0],arg_in[1],arg_in[2]); //don't need to respond. Maybe have a confirmation?
00174             bt.unlock();
00175             send_flag = 1;
00176             break;
00177 
00178         case (MOTOR_STOP):
00179             *motors.Right = 0;
00180             *motors.Left = 0;
00181             send_flag = 0;  //
00182             break;
00183             
00184         
00185         
00186         
00187         case (IMU_CALIBRATE):
00188             calibrate_flag = 1;
00189             break;
00190         case (OPT_CALIBRATE):
00191             calibrate_optFlow_flag=1;
00192             break;
00193         case END_CONNECTION:
00194             bt.lock();
00195             bt.printf("End Connection\n");
00196             bt.unlock();
00197             return 0;
00198             ///break;
00199         default:
00200             bt.lock();
00201             bt.putc(c); //echo command
00202             bt.unlock();
00203             break;
00204     }
00205     bt.lock();     
00206     bt.printf("\n\r");
00207     bt.unlock();
00208     return 1;
00209 }
00210 int ros_get_cmd(char s[16], int n)
00211 {
00212     int i= 0, j = 0;
00213     char S[16];
00214     int count = 0;
00215     while(count < n && i<13)
00216     {
00217         if(s[i] == ';')
00218             count++;
00219         i++;
00220     }
00221     count = 0;
00222     while(i<13)
00223     {
00224         if(s[i] == ';')
00225             break;
00226         S[j] = s[i];
00227         j++;
00228         i++;
00229     }
00230     for(i=0;i<j;i++)
00231     {
00232         count += ((S[i])-48)* q_pow10(j-i-1);
00233     }
00234     return count;
00235 }
00236 void ros_interface_send_data()
00237 {
00238     //int mseconds = (int)time(NULL)+(int)t.read_ms();
00239     bt.lock();            
00240     bt.printf("&%d;",t.read_ms());//indicate start of line, output system time
00241     bt.unlock();
00242     //send all the requested data
00243     //if (x.list[0]){ //send all imu data
00244         //bt.printf("A%3.3f,%3.3f,%3.3f,G%3.3f,%3.3f,%3.3f",mpu.ax,mpu.ay,mpu.az,mpu.gx,mpu.gy,mpu.gz);
00245     //}
00246     
00247         //0 - IMU_all
00248         //1 - IMU_acc
00249         //2 - IMU_gyr
00250         //3 - IMU_rotation
00251         //4 - IR_sensors
00252         //5 - Optical_flow_sensors
00253         //6 - Current_sensor
00254         //7 - Voltage_sensor
00255         
00256         
00257 }
00258 
00259 int  ros_get_cmd()   //this requires a ',' or ';' or <enter> at the end of every command
00260 {
00261     char c = '0';
00262     char C[10];     //stores the array of chars read
00263     int n=0;        //counter
00264     int timeout = 0;    //timeout counter
00265     int accum=0;  //accumulates value of array
00266     
00267    while(c != ',' && c != 13 && c !=';' && c!='x'){    //keep looping until a comma/semicolon is read (or enter is pressed)
00268         bt.lock();            
00269         while(! bt.readable()){
00270             bt.unlock();
00271             Thread::yield();   //wait for char input
00272             timeout++;
00273             if(timeout > 10000)   //if one second has passed, give up on this command
00274                 return -1;
00275             bt.lock();
00276         }            
00277         c= bt.getc();   //read the next char in the buffer
00278         bt.unlock();
00279         
00280         //bt.printf("%c",c);  //REMOVE THIS. DEBUG ONLY
00281         //if(c == '.' || n >=9) break;  //concatenate decimals. I'm not handling decimals. No reason.
00282         C[n]=c;
00283         n++;
00284    }
00285     
00286     for(int i = 0; i<n-1; i++){
00287           //need to subtract 2: 1 b/c n is incremented at the end of each iteration, 2 b/c comma is read.
00288           accum += ((C[i])-48)* q_pow10(n-i-2);
00289     }
00290   // memset(buffer, 0 , sizeof buffer);
00291     return accum;
00292 }
00293 
00294 int q_pow10(int n){     //returns 10^n, up to 10^7  [8 sig figs]
00295     static int pow10[8] = {1,10,100,1000,10000,100000,1000000,10000000};
00296     return pow10[n];
00297 }
00298 void print_all(){   //call this function to print all sensor data
00299     float data[13];
00300     data[0]= getTime();
00301     data[1]= imu_data.ypr[0];
00302     data[2]= imu_data.ypr[1];
00303     data[3]= imu_data.ypr[2];
00304     
00305     data[4]= imu2_data.ypr[0];
00306     data[5]= imu2_data.ypr[1];
00307     data[6]= imu2_data.ypr[2];
00308     
00309     
00310     bt.lock();
00311     bt.printf("&;");
00312     bt.printf("%f;",data[0]);       //print time as an integer (milliseconds)
00313     for(int i = 1; i<=6; i++){
00314             bt.printf("%.3f;",data[i]);
00315     }
00316     bt.printf("\n\r");
00317     bt.unlock();
00318     //commented out the below code because we want to ignore the rear IR sensors
00319     /*dt = t.read() - data[0];
00320     irBack = 1;
00321     if(dt < 50)
00322         Thread::wait(50-dt);
00323     
00324     data[6]= irR.read();
00325     data[7]= irL.read();
00326     data[8]= imu_data.ypr[0]*180/PI;
00327     irBack = 0;
00328     */
00329 
00330 }
00331 int IR_mm(float x)  //converts IR sensor value to millimeters
00332 {
00333     //return 9462/(x*3.3/5*1024 - 16.92);
00334     if (x <= 0.24)
00335         return -1;
00336     return 170/x;
00337 }
00338 
00339 #endif