File content as of revision 0:964eb6a2ef00:

#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