Code for our FYDP -only one IMU works right now -RTOS is working
Diff: bt_shell/machine_interface/py_machine_interface.h
- Revision:
- 0:964eb6a2ef00
diff -r 000000000000 -r 964eb6a2ef00 bt_shell/machine_interface/py_machine_interface.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/bt_shell/machine_interface/py_machine_interface.h Wed Mar 18 22:23:48 2015 +0000 @@ -0,0 +1,148 @@ +#ifndef PY_MACHINE_INTERFACE_H +#define PY_MACHINE_INTERFACE_H + +/**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 get_cmd(); //this reads from the bt connection +int q_pow10(int n); //this calculates 10^n quickly (only up to 10^6) + +char C; +char buffer[10]; + + +bool python_shell_connection(char c) +{ + float dum; + int arg_in1; + int arg_in2; + switch (c) + { + case CONNECTION: //Return query if connection is ready + bt.printf("\nEBOTDEMO\n"); + break; + + case CURRENT_SENSE: + dum = current_sensor.get_current(); + bt.printf("I%3.3f;",dum); + break; + case IMU_READ: + 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]); + break; + case IMU_YAW: + dum = imu_data.ypr[0]*180/PI; + bt.printf("y;%3.3f;",dum); + 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); + 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 + + + break; + case LOCATION: + //return x,y,z,theta (maybe not z) + break; + /*case MOTOR_R: + arg_in1 = get_cmd(); + *motors.Right = arg_in1; + case MOTOR_L: + arg_in1 = get_cmd(); + *motors.Left = arg_in1; + */ + 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_in1 = get_cmd(); + arg_in2 = get_cmd(); + *motors.Right = arg_in2; + *motors.Left = arg_in1; + bt.printf("\tL=%d\tR=%d;",arg_in1,arg_in2); + break; + case (MOTOR_STOP): + *motors.Right = 0; + *motors.Left = 0; + break; + case END_CONNECTION: + bt.printf("End Connection\n"); + return 0; + ///break; + default: + bt.putc(c); //echo command + break; + } + bt.printf("\n\r"); + return 1; +} + +void interface_send_data(interface x) +{ + bt.printf("#%f;",t.read()); //indicate start of line, output system time + //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 get_cmd() //this requires a comma 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 !=';'){ //keep looping until a comma/semicolon is read (or enter is pressed) + while(! bt.readable()){ + Thread::wait(20); //wait for char input + timeout++; + if(timeout > 100) + return -1; + } + timeout = 0; + c = bt.getc(); //read the next char in the buffer + C[n] = c; + n++; + if(c == '.' || n >=9) break; //concatenate decimals. I'm not handling decimals. No reason. + } + for(int i = 0; i<n-1; i++){ + 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. + } + 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]; +}*/ + +#endif \ No newline at end of file