File content as of revision 0:964eb6a2ef00:

#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