Code for our FYDP -only one IMU works right now -RTOS is working
Diff: bt_shell/machine_interface/ros_machine_interface.h
- Revision:
- 0:964eb6a2ef00
diff -r 000000000000 -r 964eb6a2ef00 bt_shell/machine_interface/ros_machine_interface.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/bt_shell/machine_interface/ros_machine_interface.h Wed Mar 18 22:23:48 2015 +0000 @@ -0,0 +1,339 @@ +#ifndef ROS_MACHINE_INTERFACE_H +#define ROS_MACHINE_INTERFACE_H +//THIS FILE IS INCOMPLETE/// + + +/**This file contains the implementations for the controls through a python interface +//The brain file uses ';' to separate lines +Read IMU [acceleration and gyros] +Read Gyro +Read Angle +Read Acceleration +Read IR sensors +Read Optical flow sensors +Read current sensor +Read voltage sensor + +Drive to [x,y] (x,y) +Drive fwd (speed, time) +Run R motor (speed,time) +Run L motor (speed,time) +Set Direction (direction) +Enable IMU control (bool) //if this is enabled, IMU will keep robot straight +*/ +#include "keybindings.h" + + +int ros_get_cmd(); //this reads from the bt connection +int q_pow10(int n); //this calculates 10^n quickly (only up to 10^6) +void print_all(); +int ros_get_cmd(char s[16], int n); +//char C; +//char buffer[10]; +int IR_mm(float x); + +bool ros_shell_connection(char c) +{ + char S[16]; + float dum; + float param[5]; + int arg_in[10]; + char run_time; + int m_offset = 100; //use this so that the MATLAB can send motor values between 0 and 200 instead of -100 and 100 + //bt.printf("&%f;",t.read()); + + switch (c) + { + case CONNECTION: //Return query if connection is ready + bt.lock(); + bt.printf("\nEBOTDEMO\n"); + bt.unlock(); + reckon.reset(); + break; + case ALL_SENSOR: //returns ALL sensor data //while robot is moving, send things every 50milliseconds + print_all(); + break; + case CURRENT_SENSE: + break; + case IMU_READ: + send_flag = 1; + //bt.lock(); + //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]); + //bt.unlock(); + break; + case IMU_YAW: + dum = imu_data.ypr[0]*180/PI; + bt.lock(); + bt.printf("y;%3.3f;",dum); + bt.unlock(); + break; + //optical flow + case OPT_READ: + //return optical flow (pixels/second for left and right) + //TODO: include second optical flow sensor + //bt.printf("O;%3.3f;%3.3f;",opt_flow.dx_px,opt_flow.dy_px); + bt.lock(); + 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); + bt.unlock(); + reckon.reset(); + break; + case IR_READ: + //return 5 IR sensors clockwise (Front, right_front, right_rear, left_rear, left_front) + //ask Wen to fill out this function + extern AnalogIn irL; + extern AnalogIn irC; + extern AnalogIn irR; + extern DigitalOut irBack; + if(irBack){ + irBack = 0; //read from IR + Thread::wait(50); + } + param[0] = irL.read(); + Thread::wait(10); + param[1] = irC.read(); + Thread::wait(10); + param[2] = irR.read(); + param[3] = 0; + param[4]=0; + //irBack = 1; //read back IR + /*Thread::wait(50); + param[3] = irL.read(); + Thread::wait(10); + param[4] = irR.read(); + irBack = 0;*/ + //bt.printf("%f;%f;%f;%f;%f;",param[0],param[1],param[2],param[3],param[4]); + for(int i = 0; i<5; i++) + arg_in[i]=IR_mm(param[i]); + bt.lock(); + bt.printf("%d;%d;%d;%d;%d;",arg_in[0],arg_in[1],arg_in[2],arg_in[3],arg_in[4]); + bt.unlock(); + //Thread::wait(40); + break; + /*case IR_1: + if(irBack){ + irBack = 0; + Thread::wait(50); + } + param[0] = irL.read(); + bt.printf("%c;%f;",IR_1,param[0]); + break; + case IR_2: + if(irBack){ + irBack = 0; + Thread::wait(50); + } + param[0] = irC.read(); + bt.printf("%c;%f;",IR_2,param[0]); + break; + case IR_3: + if(irBack){ + irBack = 0; + Thread::wait(50); + } + param[0] = irR.read(); + bt.printf("%c;%f;",IR_3,param[0]); + break; + case IR_4: + if(!irBack){ + irBack = 1; + Thread::wait(50); + } + param[0] = irR.read(); + bt.printf("%c;%f;",IR_4,param[0]); + break; + case IR_5: + if(!irBack){ + irBack = 1; + Thread::wait(50); + } + param[0] = irL.read(); + bt.printf("%c;%f;",IR_5,param[0]); + break;*/ + case LOCATION: + //return x,y,z,theta (maybe not z) + break; + + case MOTOR: //command: 'w[L_speed],[R_speed] //I should write an actual function to receive data from serial + //Another argument should be included: time to run motors. + arg_in[0] = ros_get_cmd(); + arg_in[1] = ros_get_cmd(); + arg_in[2] = ros_get_cmd(); + if(arg_in[0] == arg_in[1] && arg_in[0] > 100){ + if(IR_mm(irC.read()) < 260 && IR_mm(irC.read()) > 0) //if there is an obstacle, don't drive fwd + { + bt.lock(); + bt.printf("&;OBSTACLE;"); + bt.unlock(); + break; + } + fwd_flag = 1; + } + moveMotors(arg_in[0]-m_offset,arg_in[1]-m_offset,arg_in[2]); + bt.lock(); + 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? + bt.unlock(); + send_flag = 1; + break; + + case (MOTOR_STOP): + *motors.Right = 0; + *motors.Left = 0; + send_flag = 0; // + break; + + + + + case (IMU_CALIBRATE): + calibrate_flag = 1; + break; + case (OPT_CALIBRATE): + calibrate_optFlow_flag=1; + break; + case END_CONNECTION: + bt.lock(); + bt.printf("End Connection\n"); + bt.unlock(); + return 0; + ///break; + default: + bt.lock(); + bt.putc(c); //echo command + bt.unlock(); + break; + } + bt.lock(); + bt.printf("\n\r"); + bt.unlock(); + return 1; +} +int ros_get_cmd(char s[16], int n) +{ + int i= 0, j = 0; + char S[16]; + int count = 0; + while(count < n && i<13) + { + if(s[i] == ';') + count++; + i++; + } + count = 0; + while(i<13) + { + if(s[i] == ';') + break; + S[j] = s[i]; + j++; + i++; + } + for(i=0;i<j;i++) + { + count += ((S[i])-48)* q_pow10(j-i-1); + } + return count; +} +void ros_interface_send_data() +{ + //int mseconds = (int)time(NULL)+(int)t.read_ms(); + bt.lock(); + bt.printf("&%d;",t.read_ms());//indicate start of line, output system time + bt.unlock(); + //send all the requested data + //if (x.list[0]){ //send all imu data + //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); + //} + + //0 - IMU_all + //1 - IMU_acc + //2 - IMU_gyr + //3 - IMU_rotation + //4 - IR_sensors + //5 - Optical_flow_sensors + //6 - Current_sensor + //7 - Voltage_sensor + + +} + +int ros_get_cmd() //this requires a ',' or ';' or <enter> at the end of every command +{ + char c = '0'; + char C[10]; //stores the array of chars read + int n=0; //counter + int timeout = 0; //timeout counter + int accum=0; //accumulates value of array + + while(c != ',' && c != 13 && c !=';' && c!='x'){ //keep looping until a comma/semicolon is read (or enter is pressed) + bt.lock(); + while(! bt.readable()){ + bt.unlock(); + Thread::yield(); //wait for char input + timeout++; + if(timeout > 10000) //if one second has passed, give up on this command + return -1; + bt.lock(); + } + c= bt.getc(); //read the next char in the buffer + bt.unlock(); + + //bt.printf("%c",c); //REMOVE THIS. DEBUG ONLY + //if(c == '.' || n >=9) break; //concatenate decimals. I'm not handling decimals. No reason. + C[n]=c; + n++; + } + + for(int i = 0; i<n-1; i++){ + //need to subtract 2: 1 b/c n is incremented at the end of each iteration, 2 b/c comma is read. + accum += ((C[i])-48)* q_pow10(n-i-2); + } + // memset(buffer, 0 , sizeof buffer); + return accum; +} + +int q_pow10(int n){ //returns 10^n, up to 10^7 [8 sig figs] + static int pow10[8] = {1,10,100,1000,10000,100000,1000000,10000000}; + return pow10[n]; +} +void print_all(){ //call this function to print all sensor data + float data[13]; + data[0]= getTime(); + data[1]= imu_data.ypr[0]; + data[2]= imu_data.ypr[1]; + data[3]= imu_data.ypr[2]; + + data[4]= imu2_data.ypr[0]; + data[5]= imu2_data.ypr[1]; + data[6]= imu2_data.ypr[2]; + + + bt.lock(); + bt.printf("&;"); + bt.printf("%f;",data[0]); //print time as an integer (milliseconds) + for(int i = 1; i<=6; i++){ + bt.printf("%.3f;",data[i]); + } + bt.printf("\n\r"); + bt.unlock(); + //commented out the below code because we want to ignore the rear IR sensors + /*dt = t.read() - data[0]; + irBack = 1; + if(dt < 50) + Thread::wait(50-dt); + + data[6]= irR.read(); + data[7]= irL.read(); + data[8]= imu_data.ypr[0]*180/PI; + irBack = 0; + */ + +} +int IR_mm(float x) //converts IR sensor value to millimeters +{ + //return 9462/(x*3.3/5*1024 - 16.92); + if (x <= 0.24) + return -1; + return 170/x; +} + +#endif \ No newline at end of file