Mark Vandermeulen
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
--- /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