Initial Commit

Dependencies:   mbed HC05 QEI MODSERIAL SWSPI mbed-rtos

shell/bt_shell.cpp

Committer:
harshb
Date:
2014-10-21
Revision:
6:5ab1735265a9
Parent:
4:81b0de07841f

File content as of revision 6:5ab1735265a9:

#include "bt_shell.h"
//#include "keybindings.h"
//the following functions are for the python interface

//save a struct that indicates which data is to be returned
Timer tt;
interface iface;
int linear_velocity_value ;
int linear_velocity_direction;
int rotational_velocity_value ;
int rotational_velocity_direction;
int bit_size=20;
int thetha1=300;
int thetha=0;
int stall=0;
int bump=1;
int Lspeed=1;
int Rspeed=1;
int k=0;
int send_thetha=0;
int char_received=0;
DigitalOut myreset(PTA20);
Timeout reset_pgm;
//mandatory tiny shell output function
void print_all()
{
    bt.lock();
    stdio_mutex.lock();
    send_thetha=heading*11.375;
    bt.printf(">>D%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",
              dx/256,dx%256,dy/256,dy%256,\
              (send_thetha)/256,(send_thetha)%256,\
              stall, bump,\
              UL_rR/256,UL_rR%256,\
              UL_R/256,UL_R%256, \
              UL_F/256,UL_F%256,\
              UL_L/256,UL_L%256,\
              UL_rL/256,UL_rL%256,\
              UL_B/256,UL_B%256);
    dx=0;
    dy=0;
    stdio_mutex.unlock();
    bt.unlock();
}
void bt_shell_run()
{
    char buffer[12];
    bt.lock();
    if(bt.readable()) {
        bt.gets(buffer,5);
        char_received=buffer[2];
        bt.unlock();
        if(buffer[3]=='R') {
            bt.gets(buffer,char_received);
            linear_velocity_value = buffer[0]<<8|buffer[1];
            linear_velocity_direction= buffer[2];
            rotational_velocity_value = buffer[3]<<8|buffer[4];
            rotational_velocity_direction= buffer[5];

            if( linear_velocity_direction==0x01) {
                linear_velocity_value=linear_velocity_value;
                linear_velocity_value=linear_velocity_value;
            } else if( linear_velocity_direction==0x10) {
                linear_velocity_value=-linear_velocity_value;
                linear_velocity_value=-linear_velocity_value;
            }
            if( rotational_velocity_direction==0x01 && rotational_velocity_value !=0) {
                rotational_velocity_value=rotational_velocity_value*M_PI*50/180;
                Rmotor_speed=linear_velocity_value+rotational_velocity_value;
                Lmotor_speed=linear_velocity_value-rotational_velocity_value;
            } else if( rotational_velocity_direction==0x10 && rotational_velocity_value !=0) {
                rotational_velocity_value=rotational_velocity_value*M_PI*50/180;
                Rmotor_speed=linear_velocity_value-rotational_velocity_value;
                Lmotor_speed=linear_velocity_value+rotational_velocity_value;
            } else {
                Rmotor_speed=linear_velocity_value;
                Lmotor_speed=linear_velocity_value;
            }
        } else if(buffer[0] == 'P'  && buffer[1]== 'O') {
            software_interuupt=1;
            myreset=0;
        } else if(buffer[3] == 'E') {
            bt.printf(">>1B");
            bt.gets(buffer, 5);
            if (buffer[3] =='O') {
                bt.gets(buffer, 2);
                bt_connected=true;
                if(buffer[0]=='A') {
                    Selected_robot='A';
                    imperial_march();
                } else if(buffer[0]=='F') {
                    Selected_robot='F';
                    Led_on();
                }
            }
        } else if(buffer[3] == '?') {
            bt.printf("eBot#2");
        }
        else if(buffer[3] == 'X') {         // Harsh please send <<1X when you are disconnecting 
            bt_connected=false;             // from Amigobot it will realise the connection is stooped
        }
        else {
            bt.rxBufferFlush();
        }
    }
    bt.unlock();
    if(bt_connected)
        print_all();
}