EdgeBotix / Mbed 2 deprecated eBot_Firmware_V1

Initial Commit

Dependencies:   mbed HC05 QEI MODSERIAL SWSPI mbed-rtos

shell/bt_shell.cpp@3:d1197b5ea68a, 2014-10-12 (annotated)

Committer:
Throwbot
Date:
Sun Oct 12 23:27:43 2014 +0000
Revision:
3:d1197b5ea68a
Parent:
2:0f80c8a1c236
Child:
4:81b0de07841f
Ebot demo working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Throwbot 2:0f80c8a1c236 1 #include "bt_shell.h"
Throwbot 2:0f80c8a1c236 2 //#include "keybindings.h"
Throwbot 2:0f80c8a1c236 3 //the following functions are for the python interface
Throwbot 2:0f80c8a1c236 4
Throwbot 2:0f80c8a1c236 5 //save a struct that indicates which data is to be returned
Throwbot 2:0f80c8a1c236 6 Timer tt;
Throwbot 2:0f80c8a1c236 7 interface iface;
Throwbot 2:0f80c8a1c236 8 int linear_velocity_value ;
Throwbot 2:0f80c8a1c236 9 int linear_velocity_direction;
Throwbot 2:0f80c8a1c236 10 int rotational_velocity_value ;
Throwbot 2:0f80c8a1c236 11 int rotational_velocity_direction;
Throwbot 2:0f80c8a1c236 12 int bit_size=20;
Throwbot 2:0f80c8a1c236 13 int thetha1=300;
Throwbot 2:0f80c8a1c236 14 int thetha=0;
Throwbot 2:0f80c8a1c236 15 int stall=0;
Throwbot 2:0f80c8a1c236 16 int bump=1;
Throwbot 2:0f80c8a1c236 17 int Lspeed=1;
Throwbot 2:0f80c8a1c236 18 int Rspeed=1;
Throwbot 2:0f80c8a1c236 19 int k=0;
Throwbot 3:d1197b5ea68a 20 int send_thetha=0;
Throwbot 2:0f80c8a1c236 21 int char_received=0;
Throwbot 2:0f80c8a1c236 22 DigitalOut myreset(PTA20);
Throwbot 2:0f80c8a1c236 23 Timeout reset_pgm;
Throwbot 2:0f80c8a1c236 24 //mandatory tiny shell output function
Throwbot 2:0f80c8a1c236 25 void print_all()
Throwbot 2:0f80c8a1c236 26 {
Throwbot 2:0f80c8a1c236 27 bt.lock();
Throwbot 2:0f80c8a1c236 28 stdio_mutex.lock();
Throwbot 3:d1197b5ea68a 29 send_thetha=heading*11.375;
Throwbot 2:0f80c8a1c236 30 bt.printf(">>D%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",
Throwbot 2:0f80c8a1c236 31 dx/256,dx%256,dy/256,dy%256,\
Throwbot 3:d1197b5ea68a 32 (send_thetha)/256,(send_thetha)%256,\
Throwbot 2:0f80c8a1c236 33 stall, bump,\
Throwbot 2:0f80c8a1c236 34 UL_rR/256,UL_rR%256,\
Throwbot 2:0f80c8a1c236 35 UL_R/256,UL_R%256, \
Throwbot 2:0f80c8a1c236 36 UL_F/256,UL_F%256,\
Throwbot 2:0f80c8a1c236 37 UL_L/256,UL_L%256,\
Throwbot 2:0f80c8a1c236 38 UL_rL/256,UL_rL%256,\
Throwbot 2:0f80c8a1c236 39 UL_B/256,UL_B%256);
Throwbot 2:0f80c8a1c236 40 dx=0;
Throwbot 2:0f80c8a1c236 41 dy=0;
Throwbot 2:0f80c8a1c236 42 stdio_mutex.unlock();
Throwbot 2:0f80c8a1c236 43 bt.unlock();
Throwbot 2:0f80c8a1c236 44 }
Throwbot 2:0f80c8a1c236 45 void bt_shell_run()
Throwbot 2:0f80c8a1c236 46 {
Throwbot 2:0f80c8a1c236 47 char buffer[12];
Throwbot 2:0f80c8a1c236 48 bt.lock();
Throwbot 2:0f80c8a1c236 49 if(bt.readable()) {
Throwbot 2:0f80c8a1c236 50 bt.gets(buffer,5);
Throwbot 2:0f80c8a1c236 51 char_received=buffer[2];
Throwbot 2:0f80c8a1c236 52 bt.unlock();
Throwbot 2:0f80c8a1c236 53 if(buffer[3]=='R') {
Throwbot 2:0f80c8a1c236 54 bt.gets(buffer,char_received);
Throwbot 2:0f80c8a1c236 55 linear_velocity_value = buffer[0]<<8|buffer[1];
Throwbot 2:0f80c8a1c236 56 linear_velocity_direction= buffer[2];
Throwbot 2:0f80c8a1c236 57 rotational_velocity_value = buffer[3]<<8|buffer[4];
Throwbot 2:0f80c8a1c236 58 rotational_velocity_direction= buffer[5];
Throwbot 3:d1197b5ea68a 59
Throwbot 3:d1197b5ea68a 60 if( linear_velocity_direction==0x01) {
Throwbot 3:d1197b5ea68a 61 linear_velocity_value=linear_velocity_value;
Throwbot 3:d1197b5ea68a 62 linear_velocity_value=linear_velocity_value;
Throwbot 2:0f80c8a1c236 63 } else if( linear_velocity_direction==0x10) {
Throwbot 3:d1197b5ea68a 64 linear_velocity_value=-linear_velocity_value;
Throwbot 3:d1197b5ea68a 65 linear_velocity_value=-linear_velocity_value;
Throwbot 2:0f80c8a1c236 66 }
Throwbot 2:0f80c8a1c236 67 if( rotational_velocity_direction==0x01 && rotational_velocity_value !=0) {
Throwbot 3:d1197b5ea68a 68 rotational_velocity_value=rotational_velocity_value*M_PI*50/180;
Throwbot 3:d1197b5ea68a 69 Rmotor_speed=linear_velocity_value+rotational_velocity_value;
Throwbot 3:d1197b5ea68a 70 Lmotor_speed=linear_velocity_value-rotational_velocity_value;
Throwbot 2:0f80c8a1c236 71 } else if( rotational_velocity_direction==0x10 && rotational_velocity_value !=0) {
Throwbot 3:d1197b5ea68a 72 rotational_velocity_value=rotational_velocity_value*M_PI*50/180;
Throwbot 3:d1197b5ea68a 73 Rmotor_speed=linear_velocity_value-rotational_velocity_value;
Throwbot 3:d1197b5ea68a 74 Lmotor_speed=linear_velocity_value+rotational_velocity_value;
Throwbot 3:d1197b5ea68a 75 } else {
Throwbot 3:d1197b5ea68a 76 Rmotor_speed=linear_velocity_value;
Throwbot 3:d1197b5ea68a 77 Lmotor_speed=linear_velocity_value;
Throwbot 3:d1197b5ea68a 78 }
Throwbot 2:0f80c8a1c236 79 } else if(buffer[0] == 'P' && buffer[1]== 'O') {
Throwbot 3:d1197b5ea68a 80 software_interuupt=1;
Throwbot 2:0f80c8a1c236 81 myreset=0;
Throwbot 3:d1197b5ea68a 82 }
Throwbot 3:d1197b5ea68a 83 else {
Throwbot 2:0f80c8a1c236 84 bt.rxBufferFlush();
Throwbot 2:0f80c8a1c236 85 }
Throwbot 2:0f80c8a1c236 86 }
Throwbot 2:0f80c8a1c236 87 bt.unlock();
Throwbot 2:0f80c8a1c236 88 print_all();
Throwbot 2:0f80c8a1c236 89 }