BT Commander w Whiskers -- Android control for ActivityBot
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
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.
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.
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