py_machine_interface.h

00001 #ifndef PY_MACHINE_INTERFACE_H
00002 #define PY_MACHINE_INTERFACE_H
00003 
00004 /**This file contains the implementations for the controls through a python interface
00005 //The brain file uses ';' to separate lines
00006 Read IMU [acceleration and gyros]
00007 Read Gyro
00008 Read Angle
00009 Read Acceleration
00010 Read IR sensors
00011 Read Optical flow sensors
00012 Read current sensor
00013 Read voltage sensor
00014 
00015 Drive to [x,y] (x,y)
00016 Drive fwd      (speed, time)
00017 Run R motor    (speed,time)
00018 Run L motor     (speed,time)
00019 Set Direction   (direction)
00020 Enable IMU control (bool)   //if this is enabled, IMU will keep robot straight
00021 */
00022 #include "keybindings.h"
00023 
00024 int get_cmd(); //this reads from the bt connection
00025 int q_pow10(int n);     //this calculates 10^n quickly (only up to 10^6)
00026 
00027 char C;
00028 char buffer[10];
00029 
00030 
00031 bool python_shell_connection(char c)
00032 {
00033     float dum;
00034     int arg_in1;
00035     int arg_in2;
00036     switch (c)
00037     {
00038         case CONNECTION:   //Return query if connection is ready
00039             bt.printf("\nEBOTDEMO\n");
00040             break;
00041         
00042         case CURRENT_SENSE:
00043             dum = current_sensor.get_current();
00044             bt.printf("I%3.3f;",dum);
00045             break; 
00046         case IMU_READ:
00047             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]);
00048             break;
00049         case IMU_YAW:
00050             dum = imu_data.ypr[0]*180/PI;
00051             bt.printf("y;%3.3f;",dum);
00052             break;
00053         //optical flow
00054         case OPT_READ:
00055             //return optical flow (pixels/second for left and right)
00056             //TODO: include second optical flow sensor
00057             //bt.printf("O;%3.3f;%3.3f;",opt_flow.dx_px,opt_flow.dy_px);
00058             break;
00059         case IR_READ:
00060             //return 5 IR sensors clockwise (Front, right_front, right_rear, left_rear, left_front)
00061             //ask Wen to fill out this function
00062  
00063             
00064             break;
00065         case LOCATION:
00066             //return x,y,z,theta (maybe not z)
00067             break;
00068         /*case MOTOR_R:
00069             arg_in1 = get_cmd();
00070             *motors.Right = arg_in1;
00071         case MOTOR_L:
00072             arg_in1 = get_cmd();
00073             *motors.Left = arg_in1;
00074         */
00075         case MOTOR:   //command: 'w[L_speed],[R_speed]      //I should write an actual function to receive data from serial
00076             //Another argument should be included: time to run motors.
00077             arg_in1 = get_cmd();
00078             arg_in2 = get_cmd();
00079             *motors.Right = arg_in2;
00080             *motors.Left = arg_in1;
00081             bt.printf("\tL=%d\tR=%d;",arg_in1,arg_in2);
00082             break;
00083         case (MOTOR_STOP):
00084             *motors.Right = 0;
00085             *motors.Left = 0;
00086             break;
00087         case END_CONNECTION:
00088             bt.printf("End Connection\n");
00089             return 0;
00090             ///break;
00091         default:
00092             bt.putc(c); //echo command
00093             break;
00094     }
00095     bt.printf("\n\r");
00096     return 1;
00097 }
00098 
00099 void interface_send_data(interface x)
00100 {
00101     bt.printf("#%f;",t.read());    //indicate start of line, output system time
00102     //send all the requested data
00103     if (x.list[0]){ //send all imu data
00104         //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);
00105     }
00106     
00107         //0 - IMU_all
00108         //1 - IMU_acc
00109         //2 - IMU_gyr
00110         //3 - IMU_rotation
00111         //4 - IR_sensors
00112         //5 - Optical_flow_sensors
00113         //6 - Current_sensor
00114         //7 - Voltage_sensor
00115 }
00116 
00117 int get_cmd()   //this requires a comma at the end of every command
00118 {
00119     char c = '0';
00120     char C[10];     //stores the array of chars read
00121     int n=0;        //counter
00122     int timeout = 0;    //timeout counter
00123     int accum = 0;  //accumulates value of array
00124     
00125     while(c != ',' && c != 13 && c !=';'){    //keep looping until a comma/semicolon is read (or enter is pressed)
00126         while(! bt.readable()){
00127             Thread::wait(20);   //wait for char input
00128             timeout++;
00129             if(timeout > 100)
00130             return -1;
00131         }
00132         timeout = 0;
00133         c = bt.getc();   //read the next char in the buffer
00134         C[n] = c;
00135         n++;
00136         if(c == '.' || n >=9) break;  //concatenate decimals. I'm not handling decimals. No reason.
00137     }
00138     for(int i = 0; i<n-1; i++){
00139         accum += (C[i]-48)* q_pow10(n-i-2);     //need to subtract 2: 1 b/c n is incremented at the end of each iteration, 2 b/c comma is read.
00140     }
00141     return accum;
00142 }
00143 /*int q_pow10(int n){     //returns 10^n, up to 10^7  [8 sig figs]
00144     static int pow10[8] = {1,10,100,1000,10000,100000,1000000,10000000};
00145     return pow10[n];
00146 }*/
00147 
00148 #endif