Diff: bt_shell/machine_interface/py_machine_interface.h

Revision:

0:964eb6a2ef00

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