BT Commander w Whiskers -- Android control for ActivityBot
twm47099
Posts: 867
I've written an ActivityBot C program that combines the C Learning Tutorial on the Whiskers (navigating by touch) with an Android Bluetooth app "Bluetooth Joystick Commander" (see below). It is a lot simpler than the previous BT commander program I've posted that used the Pixy (CMUCAM5) color tracking camera to navigate. The only complexity is that the BT function is in a separate cog so BT data is received as soon as the joystick is moved, and the whisker function also runs in a separate cog so that the response to hitting an object is also immediate.
I've only used 2 of the 6 BT commander buttons, so the program can be expanded as you want. I've used button #5 as an emergency stop, and #6 to end the program.
I hope the code lists clearly and that you can easily use this. The code is fairly well documented.
I've noticed a problem already in that this post with the complete code is too long?? So I broke it into 2 sections (main, and the functions).
Tom
<
<
I've only used 2 of the 6 BT commander buttons, so the program can be expanded as you want. I've used button #5 as an emergency stop, and #6 to end the program.
I hope the code lists clearly and that you can easily use this. The code is fairly well documented.
I've noticed a problem already in that this post with the complete code is too long?? So I broke it into 2 sections (main, and the functions).
Tom
<
/* bt commander w whiskers.c
This uses ABot solderboard with SIRC, XBee, or BT connections and whisker detection to signal obstruction.
This program uses Joystick BT Commander (JSBTC) on pins P5 and P6. Does not use SIRC.
C code for use with Propeller ActivityBot and Joystick BT Commander android App by Kas -
(c) T. Montemarano 2015-07-02, MIT License
Tested with Samsung Tablet 4
Written for BT Commander V5 protocol
coded BT buttons: 6 = 'end' to terminate program
5 = E-stop -- moving Joystick restarts movement
4 = not defined
3 = not defined
2 = not defined
1 = not defined
Uncomment print statements and Run with terminal to debug or see command execution.
App options setup
Joystick Properties -- behavior - deselect auto return to center
-- constraint - box
Buttons Props -- display 6
-- labels -- 1- , 2- , 3- , 4- 2, 5-E stop, 6-Quit (ends program)
Advanced -- Auto connect - on
-- Refresh Interval - 100ms (works, but I am going to check out other values)
-- Timeout - Off
Uses RN42 Blutooth module. I'm using Sparkfun part that has same form as X-Bee socket
Connect BT DO to pin 6 (will be Propeller Rx), BT DI to Prop pin 5 (will be Prop Tx)
Need to pair RN42 to android BT and connect.
To run ActivityBot
Load to EEPROM, switch to 2, ABot will beep (piezo setup as in Learn examples)
After the beep, the ABot will respond to the Joystick.
Try to tap the joystick position you want, since dragging results in
a lot of data being sent to the ABot. (The app sends data when the position of the Joystick is changed)
Joystick data is x = -100 (Full Left), y=-100 (Full down), x=+100 full right, y=+100 full up.
Joystick data is sent as 02, Ascii 1st x digit, ascii 2nd x digit, ascii 3rd digit, ascii 1st y digit , etc... 03
The buttons are decoded as Ascii 'A' Button 1 lit, 'B' 1 grey, 'C' 2 lit, etc. Button code is sent when button is pressed.
Button data is sent as 02, Ascii (A to L depending on the button), 03.
(Note commas are not sent)
Example joystick data 100, 100 is sent as 2,49,48,48,49,48,48,3
The Joystick zones are setup as approx +/- 15 y = 0 speed, +/- 15 x = drive straight fwd or back.
y >15 = move fwd (JS positive), y <15 = move backwards (JS negative),
Abs(x) > approx 15 = add differential to y speed to turn or pivot (x positive -turn right).
Both x and y are scaled to set max speed and turning rates in the calculations of x2 and y2.
This can be played with to get a good range
The BT app is free on Googleplay 'joystick BT Commander". The link on the app page leads to the details
regarding the data protocols used.
BT pins:
XBEE Port Prop Pin function
DO --------- P6 ---- rxpin BT-out Prop-in
DI --------- P5 ---- txpin BT-in Prop-out
BT baud = 115200
// fdserial * fdserial_open(int rxpin, int txpin, int mode, int baudrate)
blut = fdserial_open(9, 8, 0, 115200);
*/
#include "simpletools.h"
#include "fdserial.h"
#include "abdrive.h"
// #include "ping.h"
// ***** abot ***** fspd is a variable so code can be written to allow changing max speed with JSBTC buttons
int fspd = 10; // forward speed scale. max speed = int(100/15) * fspd
/* Max Speed vs fspd
fspd: 5 10 15 20 21
max speed: 30 60 90 120 126 Note max speed for encoder contol is 128.
*/
// ***** end abot *****
// ***** BT defines *****
#define rxpin 6
#define txpin 5
#define btbaud 115200
// ***** end BTdefines *****
// ***** Other Pin Defines *****
#define spkrpin 3 // speaker pin for overlay board
#define Rwpin 8 // Right whisker pin
#define Lwpin 7 // Left whisker pin
volatile char c1[9];
volatile char btstring[] = {0, 2, '0','0','0','0','0','0',1,' ',' ','1',4,5,3}; // String to initialize BT commander buttons to off
volatile int c0 = 1;
volatile int nbytes;
volatile int joydata;
volatile int bumpflg;
int flgbt, xval, yval;
int *cogbtjoy, cogwhisker; // pointers to cog IDs
fdserial *blut; // declare bluetooth as serial device
void getbtjoy();
void whisker();
int main()
{
int x2;
int y2;
int xy;
int yspd = 0;
int xspd = 0;
int bumpflg = 0;
drive_ramp(0, 0);
cogbtjoy = cog_run(getbtjoy, 220);
cogwhisker = cog_run(whisker, 100);
freqout(spkrpin, 1000, 2000); // Speaker tone: 1 s, 2 kHz
while(c0)
{
if(nbytes == 8)
{
pause(50); // try to get rid of pause
xy = joydata; // transfer global with value from getbtjoy() to local
if( ! bumpflg)
{
y2 = (xy & 511) - 200; // unpack y joystick value
x2 = (xy >> 10) - 200; // unpack x
printi("x = %d\n", x2);
printi("y = %d\n", y2);
yspd = (abs(y2) / 15) * fspd; // scale the fwd & rev motion was *15 -- try *10
if(y2 <0) yspd = yspd / -2; // go slower in reverse
xspd = (abs(x2) / 15) * 4; // scale the turning
if(x2<0) xspd = xspd * -1;
printi(" left right %d %d\n",yspd+xspd, yspd-xspd);
drive_ramp(yspd+xspd, yspd-xspd); // drive, turning differential added to yspd
} // end if not bumpflg -- if whisker is hit preceeding group is not executed, but serial port runs normally
// there should be no need to flush BT serial buffer when whisker is done.
}
if(nbytes == 3) // button pressed
{
nbytes = 0;
printi(" button = %d\n\n", c1[2]);
} // end button pressed
} // end while c0
drive_ramp(0, 0); // stop movement at end of program
// printi(" end \n\n");
cog_end(cogwhisker); // close whisker cog
cog_end(cogbtjoy); // end bt
freqout(spkrpin, 200, 2000); // Speaker tone: 0.2 s, 2 kHz
} // end main
<
Comments
void getbtjoy() { // fdserial * fdserial_open(int rxpin, int txpin, int mode, int baudrate) blut = fdserial_open(rxpin, txpin, 0, btbaud); for(int q =1; q<=14; q++) { fdserial_txChar(blut, btstring[q]) ; } // send string to set all BT app buttons off freqout(4, 1000, 2000); // Speaker tone: 1 s, 2 kHz while(c0) // continue until button 6 - quit - is pressed { nbytes = 0; int i = 1; flgbt= 1; c1[1] = fdserial_rxChar(blut); // read first byte from blut if(c1[1] == 2) // if STX read next 7 bytes { flgbt = 1; while(flgbt) { i++; c1[i] = fdserial_rxChar(blut); if((c1[i]==3) && (i==3 || i==8)) flgbt=0; } // end while flgbt, android string i = 3 => button, i = 8 => x,y values if(i==8) // decode joystick x, y { nbytes = 8; // used in main, means got joystick value xval = (c1[2] - 48) * 100 + (c1[3] - 48) * 10 + (c1[4] - 48); // make number from ascii digits yval = (c1[5] - 48) * 100 + (c1[6] - 48) * 10 + (c1[7] - 48); joydata=(xval << 10) | yval; // pack x & y into one global variable } // end if i= 8 if(i == 3) // decode button { nbytes = 3; // used in main, means got button value // printi(" button = %d\n\n", c1[2]); } if((i==3) && (c1[2]=='K' || c1[2]=='L')) // Button 6, ends program { c0 = 0; //printi(" end \n\n"); } if((i==3) && (c1[2]=='I' || c1[2]=='J')) { //E-stop on button 5 press, using JS restarts movement drive_ramp(0, 0); } } // end if c1=2 } // end while c0 } // end getbtjoy void whisker() // need to 1. stop bot, 2. disable JS control, 3. back up, 4. turn away from obstacle, 5. stop, // 6. clear JS buffer, 7. restore JS control { while(1) // runs continuously { // If whisker pressed, avoid obstacle. // First just right whisker - input(8) is 0 if whisker is pressed if(! input(Rwpin)) { bumpflg = 1; // Set flag so main will not give conflicting drive commands drive_speed(0,0); // stop drive_ramp(-60, -60); // Back up 0.5 seconds pause(500); drive_ramp(0,0); drive_speed(-26, 26); // Turn left 1 seconds -- 90 deg pause(1000); drive_ramp(0, 0); // stop } // Just left whisker - input(7) else if(! input(Lwpin)) { drive_speed(0,0); bumpflg = 1; drive_ramp(-60, -60); // Back up 0.5 seconds pause(500); drive_ramp(0,0); drive_speed(26, -26); // Turn right 1 seconds -- 90 deg pause(1000); drive_ramp(0,0); // stop } bumpflg = 0; // clear flag, allow main to have control } }Tom
Just open a new C project in SimpleIDE. Delete the code automatically entered into a new project and copy the code in the first post -- this has the main() program -- into the blank project. Then copy the code in the second post (functions) below the main.
Tom
In the area where I declared the volatile variables there is this statement.
[code]After the variable name "btstring" there should be square brackets with nothing between them. In the code block, the forum changed the brackets to a rectangle.
Tom
Tom
1. I changed the whisker function to use "waitpne" which stalls the whisker cog until one of the whiskers is pressed without doing the constant polling.
2. I moved the bump flag test in main() to the statement that actually did the drive command to minimize the chance that JS control would conflict with whisker control.
3. I added the ability to change the top speed of the bot by using buttons 3 and 4. Press 3 to increase the scaling and 4 to lower it. The change takes effect on the next JS tap. There are 5 speed scale settings. The index of the scale array is shown on the android device (lower left) and ranges from 0 to 4. The program starts at index 2 (max speed ~1 rps wheel speed). When you reach the limit for the button (0 for button 4, 4 for button 3), the button label on the android device dims. You can change the values I used for the speed scale by changing the numbers in the fspd[] array.
/* bt commander w whiskers wait b3-4.c This uses ABot solderboard with SIRC, XBee, or BT connections and whisker detection to signal obstruction. This program uses Joystick BT Commander (JSBTC) on pins P5 and P6. Does not use SIRC. This version uses waitpne in whisker function, and uses buttons 3 and 4 to change maximum possible speed (24 to 102 ticks/sec) C code for use with Propeller ActivityBot and Joystick BT Commander android App by Kas - (c) T. Montemarano 2015-07-02, MIT License Tested with Samsung Tablet 4 Written for BT Commander V5 protocol coded BT buttons: 6 = 'end' to terminate program 5 = E-stop -- moving Joystick restarts movement 4 = decrease maximum speed with next joystick tap 3 = increase maximum speed with next joystick tap 2 = not defined 1 = not defined Uncomment print statements and Run with terminal to debug or see command execution. App options setup Joystick (JS) Properties -- behavior - deselect auto return to center -- constraint - box Buttons Props -- display 6 -- labels -- 1- , 2- , 3-up, 4-down, 5-E stop, 6-Quit (ends program) Advanced -- Auto connect - on -- Refresh Interval - 100ms (works, but I am going to check out other values) -- Timeout - Off Uses RN42 Blutooth module. I'm using Sparkfun part that has same form as X-Bee socket Connect BT DO to pin 6 (will be Propeller Rx), BT DI to Prop pin 5 (will be Prop Tx) Need to pair RN42 to android BT and connect. To run ActivityBot Load to EEPROM, switch to 2, ABot will beep (piezo setup as in Learn examples) After the beep, the ABot will respond to the Joystick. Try to tap the JS position you want, since dragging results in a lot of data being sent to the ABot. (The app sends data when the position of the JS is changed) JS data is x = -100 (Full Left), y=-100 (Full down), x=+100 full right, y=+100 full up. JS data sent as 02, Ascii 1st x digit, ascii 2nd x digit, ascii 3rd digit, ascii 1st y digit , etc... 03 The buttons are decoded as Ascii 'A' Button 1 lit, 'B' 1 grey, 'C' 2 lit, etc. Button code is sent when button is tapped. Button data is sent as 02, Ascii (A to L depending on the button), 03. (Note commas are not sent) Example: JS data 100, 100 is sent as 2,49,48,48,49,48,48,3 The JS zones are setup as approx +/- 15 y = 0 speed, +/- 15 x = drive straight fwd or back. y >15 = move fwd (JS positive), y <15 = move backwards (JS negative), Abs(x) > approx 15 = add differential to y speed to turn or pivot (x positive -turn right). Both x and y are scaled to set max speed and turning rates in the calculations of x2 and y2. This can be played with to get a good range The BT app is free on Googleplay 'Joystick BT Commander". The link on the app page leads to the details regarding the data protocols used. BT pins: XBEE Port Prop Pin function DO ----- P6 ---- rxpin BT-out Prop-in DI ----- P5 ---- txpin BT-in Prop-out BT baud = 115200 // fdserial * fdserial_open(int rxpin, int txpin, int mode, int baudrate) blut = fdserial_open(9, 8, 0, 115200); */ #include "simpletools.h" #include "fdserial.h" #include "abdrive.h" // ***** BT defines ***** #define rxpin 6 #define txpin 5 #define btbaud 115200 // ***** end BTdefines ***** // ***** Other Pin Defines ***** #define spkrpin 3 // speaker pin for overlay board #define Rwpin 8 // Right whisker pin #define Lwpin 7 // Left whisker pin // ***** end pin defines ***** // fspd is a variable array so code can be written to allow changing max speed with JSBTC buttons int fspd[]= {5, 8, 11, 13, 16}; // forward speed scale. max speed = int(100/15) * fspd /* Max Speed vs fspd fspd: 5 8 11 13 16 max speed: 30 48 66 78 96 Note max speed for encoder contol (yspd + xspd) is 128. max rps: 0.5 0.8 1 1.2 1.5 */ // ***** end fspd ***** volatile char c1[9]; volatile char btstring[] = {0, 2, '0','0','1','1','0','0',1,' ',' ','2',4,5,3}; // btsring[] -- String to initialize BT commander buttons 3 & 4 lit // button states are ascii binary digits from right to left. // button 1 is the '0' to the left of the number 1, button 6 is '0' to right of the number 2 volatile int c0 = 1; volatile int nbytes; volatile int joydata; volatile int bumpflg; volatile int fspdi = 2; // index to fspd[] array, starts at middle value int flgbt, xval, yval; int *cogbtjoy, cogwhisker; // pointers to cog IDs fdserial *blut; // declare bluetooth as serial device void getbtjoy(); // get & decode serial data from JSBTC, pass data to main with globals void whisker(); // monitors whiskers, if hit: stops, backs away, turns away void abuttons(); // use this for non-immediate buttons (quit and E-stop are immediate buttons) int main() { int x2; int y2; int xy; int yspd = 0; int xspd = 0; bumpflg = 0; drive_ramp(0, 0); // make sure robot is stopped at start cogbtjoy = cog_run(getbtjoy, 220); // start cog to get serial data from JSBTC cogwhisker = cog_run(whisker, 100); // start monitoring whiskers freqout(spkrpin, 1000, 2000); // Speaker tone: 1 s, 2 kHz while(c0) { if(nbytes ==8) // Joystick data received { pause(50); xy = joydata; // transfer global with value from getbtjoy() to local y2 = (xy & 511) - 200; // unpack y joystick value x2 = (xy >> 10) - 200; // unpack x printi("x = %d\n", x2); printi("y = %d\n", y2); yspd = (abs(y2) / 15) * fspd[fspdi]; // scale the fwd & rev motion if(y2 <0) yspd = yspd / -2; // go slower in reverse xspd = (abs(x2) / 15) * 4; // scale the turning if(x2<0) xspd = xspd * -1; printi(" left right %d %d\n",yspd+xspd, yspd-xspd); if( ! bumpflg) // whiskers not controlling movement { drive_ramp(yspd+xspd, yspd-xspd); // drive, turning differential added to yspd } // end if not bumpflg -- if whisker is hit preceeding drive_ramp is not executed, // but serial port runs normally // there should be no need to flush BT serial buffer when whisker is done. } // end nbytes==8 if(nbytes == 3) // button pressed { nbytes = 0; printi(" button = %d\n\n", c1[2]); abuttons(); printi("fspdi = %d\n", fspdi); btstring[11] = 0x30 + fspdi; // show value of index on android device for(int q =1; q<=14; q++) { fdserial_txChar(blut, btstring[q]) ; } // send string to BT } // end button pressed } // end while c0, Quit button pressed, program ending drive_ramp(0, 0); // stop movement at end of program // printi(" end \n\n"); cog_end(cogwhisker); // close whisker cog cog_end(cogbtjoy); // close bt cog freqout(spkrpin, 200, 2000); // Speaker tone: 0.2 s, 2 kHz } // end main void getbtjoy() { // fdserial * fdserial_open(int rxpin, int txpin, int mode, int baudrate) blut = fdserial_open(rxpin, txpin, 0, btbaud); for(int q =1; q<=14; q++) fdserial_txChar(blut, btstring[q]); // light BT app buttons 3 and 4 while(c0) // continue until button 6 - quit - is pressed { nbytes = 0; int i = 1; flgbt= 1; c1[1] = fdserial_rxChar(blut); // read first byte from blut if(c1[1] == 2) // if STX read next 7 bytes { flgbt = 1; while(flgbt) { i++; c1[i] = fdserial_rxChar(blut); if((c1[i]==3) && (i==3 || i==8)) flgbt=0; } // end while flgbt, android string i = 3 => button, i = 8 => x,y values if(i==8) // decode joystick x, y { nbytes = 8; // used in main, means got joystick value xval = (c1[2] - 48) * 100 + (c1[3] - 48) * 10 + (c1[4] - 48); // make number from ascii digits yval = (c1[5] - 48) * 100 + (c1[6] - 48) * 10 + (c1[7] - 48); joydata=(xval << 10) | yval; // pack x & y into one global variable to be used by cog 0 } // end if i= 8 if(i == 3) // decode button { nbytes = 3; // used in main, means got button value // printi(" button = %d\n\n", c1[2]); } // ***** These are immediate buttons take action now ***** if((i==3) && (c1[2]=='K' || c1[2]=='L')) // Button 6, ends program { c0 = 0; //printi(" end \n\n"); } if((i==3) && (c1[2]=='I' || c1[2]=='J')) { //E-stop on button 5 press, using JS restarts movement drive_ramp(0, 0); } // ***** End immediate buttons ***** } // end if c1=2 } // end while c0 } // end getbtjoy void whisker() // If whisker pressed, stop & move away from obstacle: // 1. stop bot, 2. disable JS control, 3. back up, 4. turn away from obstacle, // 5. stop, 6. clear JS buffer, 7. restore JS control { // uses waitpne(wstate, wmask) int wmask = 0b0110000000; // pins 7 & 8, hex 180 int wstate = 0x180; // in normal running both pins are high, hit whisker pin drops to low int RLval; while(1) // runs continuously { waitpne(wstate, wmask); //stop cog until a whisker is pressed RLval = get_states(Rwpin, Lwpin); // get whisker pin values bumpflg = 1; // Set flag so main will not give conflicting drive commands drive_speed(0,0); // stop bot drive_ramp(-60, -60); // Back up 0.5 seconds pause(500); drive_ramp(0,0); // and stop // ***** Check direction to turn ***** switch(RLval) // decode whiskers 0b00=both, 0b01=right, 0b10=left, 0b11=neither { case 0 : // ** both whiskers hit, turn right drive_speed(26, -26); // Turn right 1 seconds -- 90 deg break; case 1 : // ** Right whisker hit, turn left drive_speed(-26, 26); // Turn left 1 seconds -- 90 deg break; case 2 : // ** Left whisker hit, turn right drive_speed(26, -26); // Turn right 1 seconds -- 90 deg break; case 3 : // ** Neither whisker hit, don't turn break; } // end switch // now do the 1 second delay for the 90 degree turn pause(1000); drive_ramp(0, 0); // and stop bumpflg = 0; // clear flag, allow main to have control } // end while } // end whisker function void abuttons() { if((c1[2]=='E' || c1[2]=='F')) // increase top speed (fspd[]) { // index maximum value of 4. Does not take effect until next JS tap if(fspdi < 4) fspdi ++; } if((c1[2]=='G' || c1[2]=='H')) // decrease top speed, index minimum value of 0 { if(fspdi > 0) fspdi --; } btstring[4] = '1'; btstring[5] = '1'; if(fspdi == 4) btstring[5] = '0'; // if index is 4 dim Up button label if(fspdi == 0) btstring[4] = '0'; // if index is 0 dim Down label return; }The button states are ascii binary digits (1 = bright, 0 = dim) from right to left. In the example below from the above program,button 1 is the '0' to the left of the number 1, button 6 is '0' to right of number 2.
volatile char btstring[] = {0, 2, '0','0','1','1','0','0',1,' ',' ','2',4,5,3};The first is an overall shot of the ActivityBot showing the overlay board with wiring, the Sparkfun BT radio in the XBee socket, and the whisker mount.
The second shot is the details of the overlay board board wiring. The device on the left middle of the board is the Sony IR Code receiver. It is not used in this program, but I use the overlay board for different but related programs.
The color codes I used for this are Red=3.3v, Orange = 5v, Black = ground, Blue on the board = SIRC signal, Blue jumper = BT In-Prop Out, Purple & Green = whisker pins, Yellow on board = piezio, Yellow jumper = BT out-Prop In.
First shows the location of the ActivityBot encoder resistors. I insert them into the ABot breadboard as discribed in the ABot instructions. When I add the headers to the overlay board, I don't put any in the P14, P15, and two 3.3v positions where the resistors are inserted. That way the overlay board just fits on top.
Last shot -- There was an issue when using an overlay board and the whiskers. The overlay board sits too high to use the whisker mounting sjown in the tutorial, and they could also contact the A/D or 5V header pins on the overlay board. So I used a parallax universal mounting bracket on each side attached to the ABoard standoff, and sandwiched the whisker between 2 nuts on a bolt just ahead of the standoff. Note that the bracket on this side is attached under the nylon washer. On the other side it is attached on top of the washer to give vertical clearance between the 2 whiskers.
However, it seems that new code entered after the forum update will be ok, so I'll have to reenter the code (in a couple of days).
Tom
In print statements, where ever I used the \n escape sequence to make a line feed, the forum software deleted the \n and actually inserted a line feed in the code. I was going to wait until the forum team fixed it, but since the weekend is here, I'll try to fix it Saturday. I may just delete the print statements since I only use them for debugging.
Tom
Tom