Bluetooth App to control ActivityBot

twm47099twm47099 Posts: 553
Now that I have had my questions answered, I'd like to post a C-program that interfaces with a free android bluetooth joystick app to control the ActivityBot. The app is "Joystick BT Commander" by "Kas". It can be downloaded from GooglePlay. I'm using it on a Samsung Tablet 4, but the author says it works on a number of phones and tablets.

The link to the app is here:

https://play.google.com/store/apps/details?id=org.projectproto.btjoystick

The program below is setup to drive the ActivityBot using the joystick. There are 6 soft buttons on the app that can be used for any user defined function. I use 3 of them in the program to: end the program, as an emergency stop, and to turn ping on and off. The ping function is an example that simply lights the red and green channels of an RBG Led to show distance in front of the Bot to an object (<20cm = red, etc).

I intend to add my PIXY color tracking code as a function to be turned on and off using another button. When on, that code will take over driving the bot to go to a predefined colored object. The pixy code is at post 27 in:
http://forums.parallax.com/showthread.php/155501-Pixy-for-Propeller?p=1279736&highlight=pixy#post1279736

The comments in the code below explain the hardware connections and the BT commander data format. More info is available at the links on the app page on googleplay.

I hope that you find this interesting and something you can expand on.

Tom
/* C code for use with Propeller ActivityBot and Joystick BT Commander android App by Kas -

(c) T. Montemarano 2014-08-01,  MIT License

Tested with Samsung Tablet 4

2014-08-13 Deleted & from function name in cog_run statement, added 'flgping' flag to show when ping is running.

2014-08-12 Made global declaration of *cogping, increased cog stack sizes, fixed call to do_ping

2014-08-09  used 'volatile' in global declarations.
Written for BT Commander V5 - Changed protocol -- need to do data TX to android
coded BT button 6 as 'end' to terminate program
coded BT button 5 as E-stop  -- moving Joystick restarts movement
coded BT button 2 to turn ping on and off
Uncomment print statements and Run with terminal to debug or see command execution.

App options setup
Joystick Props -- behavior - deselect auto return to center
constraint - box
Buttons Props -- display 6
labels -- 1, 2 ping, 3, 4, - optional.  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 9 (Will be Propeller Rx), BT DI to Prop pin 8 (will be Prop Tx)
Need to pair RN42 to android BT and connect (with ABot switch in position 1)
To run ActivityBot
Load to EEPROM, switch to 2, ABot will beep (piezo setup as in Learn examples)
After the beep and a second or so, 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.
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.

The Joystick zones are setup as approx +/- 15 y = 0 speed, +/- 15 x = drive straight fwd or back.
abs(y) >15 = move fwd (JS positive), 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
Ping)) has signal connected to pin 5 with 2kohm resistor in series
common cathode rbg led has red to pin 6 with 100 ohm resistor, green to pin 7 with 120 ohm resistor, cathode to gnd.

To do: add pixy color tracking (use BT button 3) when active pixy tracking takes control and drives aBot to defined color object.

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.
*/

#include "simpletools.h"
#include "fdserial.h"
#include "abdrive.h"
#include "ping.h"

volatile char c1[9];
volatile int c0 = 1;
volatile int nbytes;
volatile int joydata;
int flgping;
int xval;
int yval;
int flgbt;
int dist;
int *cogping;

fdserial *blut;

void getbtjoy();
void do_ping();

int main()
{
freqout(4, 1000, 2000); // Speaker tone: 1 s, 2 kHz
int x2;
int y2;
int xy;
int yspd = 0;
int xspd = 0;
flgping = 0;                //    ping cog is not running
cog_run(getbtjoy, 220);
drive_ramp(0, 0);
while(c0)
{
if(nbytes == 8 )
{
pause(50);        //   try to get rid of pause
xy = joydata;
y2 = (xy & 511) - 200;
x2 = (xy >> 10) - 200;
printi("x = %d\n", x2);
printi("y = %d\n", y2);
yspd = (abs(y2) / 15) * 15;         // 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);
drive_ramp(yspd+xspd, yspd-xspd);         // drive, turning differential added to yspd
}
if(nbytes == 3)
{
nbytes = 0;
printi(" button = %d\n\n", c1[2]);
if(c1[2]=='C')                  // Button 2, on turns on ping
{
cogping = cog_run(do_ping,120);
printi(" ping cog = %d\n\n", *cogping);
nbytes = 0;
flgping = 1;                      // set flgping -- ping cog is running
}
if(c1[2]=='D' && flgping)                // Button 2, off turns off ping, don't try to stop if not running
{
cog_end(cogping);
nbytes = 0;
flgping = 0;                      // clear flgping -- ping cog is stopped
}
}
} // end while c0
drive_ramp(0, 0);     // stop movement at end of program
//  printi(" end \n\n");
if(flgping) cog_end(cogping);       //  if ping running when quit is pressed, turn off
} // end main

void getbtjoy()
{
// fdserial * fdserial_open(int rxpin, int txpin, int mode, int baudrate)
blut = fdserial_open(9, 8, 0, 115200);
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 = 2 => button, i = 7 => x,y values
if(i==8)
{
nbytes = 8;
xval = (c1[2] - 48) * 100 + (c1[3] - 48) * 10 + (c1[4] - 48);
yval = (c1[5] - 48) * 100 + (c1[6] - 48) * 10 + (c1[7] - 48);
joydata=(xval << 10) | yval;
} // end if i= 8
if(i==3)
{
nbytes = 3;
// 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 do_ping()
{
while(1)
{
dist = ping_cm(5);
if(dist<=20)            // turn on red LED
{
high(6);
low(7);
}
else if(dist<=36)      // turn on yellow LED
{
high(6);
high(7);
}
else                    //  turn on green LED
{
low(6);
high(7);
}
pause(50);
}   // end while
}


Comments

  • 24 Comments sorted by Date Added Votes
  • ValeTValeT Posts: 308
    edited August 2014 Vote Up0Vote Down
    This is great!

    Thanks for the code. I am starting a project right now that has to do with Bluetooth and using a mobile device/PC to talk to my Arlo, and it is not easy to do with Java. Hopefully this will lead me in the right direction.

    Please keep me posted!
  • NWCCTVNWCCTV Posts: 3,629
    edited August 2014 Vote Up0Vote Down
    Any chance this will work with an Easy Bluetooth Module?
    Andy North

    My Index Page:
  • twm47099twm47099 Posts: 553
    edited August 2014 Vote Up0Vote Down
    NWCCTV wrote: »
    Any chance this will work with an Easy Bluetooth Module?

    Andy,
    I'm not sure, but I think it will if the EasyBT Din and Dout are connected to the pins noted in the comments in the code in my first post. The only other thing is that I use 115,200 baud in the beginning of the getbtjoy() function:
    void getbtjoy() {
       // fdserial * fdserial_open(int rxpin, int txpin, int mode, int baudrate)
       blut = fdserial_open(9, 8, 0, 115200);
    

    I don't know if the easyBT module can use that baud. If not just change the value in the fdserial_open statement.
    (115,200 was the default baud for the RN42 module, and I didn't feel like changing it.)

    Please let me know if it works.

    Tom
  • twm47099twm47099 Posts: 553
    edited August 2014 Vote Up0Vote Down
    I've added the functions for using the Pixy (CMUCAM5) color tracking module to the Bluetooth - ActivityBot control program.

    In the code below, turning on soft button #3 turns off the ping LED function, disables joystick control, and turns Pixy control on.
    The way I use it is to teach Pixy the color of the object I want it to find. Drive the ActivityBot to a location where the object can be seen by the Pixy, although not in the field of view of the lens. Then I press soft button #3 until it lights (if already lit I press 2xs). The ABot beeps and then starts to search for the object. When it locates it, the Bot will travel towards it, correcting its approach as it gets closer. When it gets within a couple of inches, the Bot will beep 3xs and stop. Turning button 3 off will reenable the joystick and the Bot can again be manually driven. Pressing button will end he program.

    I have Ping mounted at the front of my bot on the pring backet and the Pixy on top of the Ping bracket using a small piece of stiff plastic sheet. To rebalance the Bot, I've added the Speaker to the back, but some weights hangingoff the rear standoffs also works.

    All of the standard Pixy issues remain - I've found it best to train Pixy using the push-button method under the same lighting as will be used for tracking. It uses Pixy interface mode 3 -- voltage corresponding to x-position of largest object
    Pixypin 1 -- abot adc0, Ppin2 -- +5v, Ppin3 -- adc1, Ppin6 -- gnd.
    Color needs to be unique and large enough to see at a distance. Laser pointer dot is too small.


    I've got some more optimizations to do, but this works. The code follows:

    Tom
    /* C code for use with Propeller ActivityBot and Joystick BT Commander android App by Kas -  
    
     (c) T. Montemarano 2014-08-01,  MIT License
      Tested with Samsung Tablet 4
     2014 -08-18 Added code for Pixy
     2014-08-13 Deleted & from function name in cog_run statement, added 'flgping' flag to show when ping is running.
     2014-08-12 Made global declaration of *cogping, increased cog stack sizes, fixed call to do_ping
     2014-08-09  used 'volatile' in global declarations.
     Written for BT Commander V5 - Changed protocol -- need to do data TX to android
     coded BT button 6 as 'end' to terminate program
     coded BT button 5 as E-stop  -- moving Joystick restarts movement
     coded BT button 3 as Pixy on and off
     coded BT button 2 to turn ping on and off
     Uncomment print statements and Run with terminal to debug or see command execution.
     Using the code as-is running with terminal shows the joystick x & y position and the right and left wheel speed.
     It also shows the value of a button press and the cog number to which ping or pixy is assigned.
      
     App options setup
         Joystick Props -- behavior - deselect auto return to center
                                 constraint - box
         Buttons Props -- display 6
                                 labels -- 1, 2 ping, 3 pixy, 4, - optional.  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 9 (Will be Propeller Rx), BT DI to Prop pin 8 (will be Prop Tx)
     Need to pair RN42 to android BT and connect (with ABot switch in position 1)
     To run ActivityBot 
     Load to EEPROM, switch to 2, ABot will beep (piezo setup as in Learn examples)
     After the beep and a second or so, 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.
       The data is sent as 3 ascii values for x and 3 for y, each value corresponding to a digit.
       e.g 100 is sent as 49, 48, 48
     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.
      
     The Joystick zones are setup as approx +/- 15 y = 0 speed, +/- 15 x = drive straight fwd or back.
     abs(y) >15 = move fwd (JS positive), 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
     Ping)) has signal connected to pin 5 with 2kohm resistor in series
     common cathode rbg led has red to pin 6 with 100 ohm resistor, green to pin 7 with 120 ohm resistor, cathode to gnd.
     
    Uses Pixy interface mode 3 -- voltage corresponding to x-position of largest object
      Pixypin 1 -- abot adc0, Ppin2 -- +5v, Ppin3 -- adc1, Ppin6 -- gnd
      Color needs to be unique and large enough to see at a distance.  Laser pointer dot is too small.
      
     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.
     */
      
     #include "simpletools.h" 
     #include "fdserial.h"
     #include "abdrive.h"
     #include "ping.h"
     #include "adcDCpropab.h"              // Include adcDCpropab
      
     #define rspd 3                        //  speed to rotate L or R when searching for color  was 3
     #define fspd 30                       // forward speed
      
     volatile char c1[9];
     volatile int c0 = 1;
     volatile int nbytes;   
     volatile int joydata;
     int flgping;  
     int xval; 
     int yval; 
     int flgbt; 
     int flgpixy;   
     int dist;
     float adc0;
     float adc1;                               // Voltage variables
     int dir = -1;                             //  -1 = left, +1 = right
     int dist1;                                 // Declare distance variable
     int notdone = 1;
     int *cogbtjoy; 
     int *cogping;
     int *cogpixy;
      
     fdserial *blut;
      
     void getbtjoy();
     void do_ping();
     void findsig();
     void pixyrun();
      
     int main()
     {
       freqout(4, 1000, 2000); // Speaker tone: 1 s, 2 kHz
       int x2;
       int y2;
       int xy;
       int yspd = 0;
       int xspd = 0;
       flgping = flgpixy = 0;                //    ping cog is not running, pixy not running
       cogbtjoy = cog_run(getbtjoy, 220);  
       drive_ramp(0, 0);
       while(c0)
       {
         if((nbytes == 8) && (flgpixy == 0)) 
         {
           pause(50);        //   try to get rid of pause
           xy = joydata;
           y2 = (xy & 511) - 200;
           x2 = (xy >> 10) - 200;
           printi("x = %d\n", x2);
           printi("y = %d\n", y2);
           yspd = (abs(y2) / 15) * 15;         // 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);
           drive_ramp(yspd+xspd, yspd-xspd);         // drive, turning differential added to yspd
         }
         if(nbytes == 3) 
         {
           nbytes = 0;
           printi(" button = %d\n\n", c1[2]);
           if((c1[2]=='C' )&& (flgping == 0))                  // Button 2, on turns on ping
           {
             if(flgpixy)                                           // If pixy function is running stop its cog
              { 
               cog_end(cogpixy);
               flgpixy = 0;
              }        
             cogping = cog_run(do_ping,120);  
             printi(" ping cog = %d\n\n", *cogping);
             nbytes = 0;
             flgping = 1;                      // set flgping -- ping cog is running
           }
           if(c1[2]=='D' && flgping)                // Button 2, off turns off ping, don't try to stop if not running
           {
             cog_end(cogping);
             nbytes = 0;
             flgping = 0;                      // clear flgping -- ping cog is stopped
           }
           if((c1[2]=='E') && (flgpixy ==0))                  // Button 3, on turns on pixy, turn off ping cog
           {
             if(flgping) 
             {
               cog_end(cogping);       //  if ping running when pixy turned on, turn off
               flgping = 0;
             }
             notdone = 1;
             cogpixy = cog_run(pixyrun,120);  
             printi(" pixy cog = %d\n\n", *cogpixy);
             nbytes = 0;
             flgpixy = 1;                      // set flgpixy -- pixy is running
           }
           if(c1[2]=='F' && flgpixy)                // Button 3, off turns offpixy, don't try to stop if not running
           {
             notdone = 0;
             drive_ramp(0,0); 
             cog_end(cogpixy);
             nbytes = 0;
             flgpixy = 0;                      // clear flgpixy -- pixy cog is stopped
           }
         }
       } // end while c0
       drive_ramp(0, 0);     // stop movement at end of program
       //  printi(" end \n\n");
         if(flgping) cog_end(cogping);       //  if ping running when quit is pressed, turn off
         if(flgpixy) cog_end(cogpixy);       // if pixy running when quit is pressed, turn off
         cog_end(cogbtjoy);                  // end bt
     } // end main
      
     void getbtjoy()
     {
       // fdserial * fdserial_open(int rxpin, int txpin, int mode, int baudrate)
       blut = fdserial_open(9, 8, 0, 115200); 
       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)
           {
             nbytes = 8;
             xval = (c1[2] - 48) * 100 + (c1[3] - 48) * 10 + (c1[4] - 48);
             yval = (c1[5] - 48) * 100 + (c1[6] - 48) * 10 + (c1[7] - 48);
             joydata=(xval << 10) | yval;
           } // end if i= 8
           if(i==3) 
           {
             nbytes = 3;
             // 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 do_ping() 
     {
       while(1)
       {
         dist = ping_cm(5);
         if(dist<=20)            // turn on red LED
         {
           high(6);
           low(7);
         }
         else if(dist<=36)      // turn on yellow LED
         {
           high(6);
           high(7);
         }
         else                    //  turn on green LED
         {
           low(6);
           high(7);
         }
         pause(50);
       }   // end while
     }
      
     void pixyrun()                               // Main pixy function
     {
      freqout(4, 2000, 3500);                       // Speaker tone: 2 s, 3.5 kHz
      adc_init(21, 20, 19, 18);               // CS=21, SCL=20, DO=19, DI=18
      drive_setRampStep(1);               // 4 ticks per 50th of sec is default
      while(notdone)                                // Loop repeats indefinitely
       {
         adc0= adc_volts(0);
      printi("%f\n",adc0);              // *** for debug
         if(adc0 <2)                     // Pixy pin 1 = low -- no objct detected
         {
          drive_ramp(0,0);   // Stop - color signature out of pixy FOV, approx +/- 37.5 deg.
          findsig();          // rotate activitybot to find color
         }
          else 
          {
           adc1 = adc_volts(1);
       printi("%f\n", adc1);          // *** for debug
           if(adc1<1.3)                //  color sig to left of center
           {
             dir = -1;
             drive_speed(-rspd,rspd);
             pause(30);   // was 100  ****
           }
            else if(adc1>1.9)         //  color sig to right of center
            {
             dir = 1;
             drive_speed(rspd,-rspd);
             pause(30);   //  was 100 ****
            }
            else if((adc1 <=1.9) && (adc1 >= 1.3))      //  Color sig at center, play with limits 
            {
             drive_ramp(fspd,fspd);
             pause(100);
            }
          }             //  end of main if-else
        pause(200); 
        dist1 = ping_inches(5);                    // Get inch distance from Ping)))
        if(dist1<4)                             //  less than 3 inches -- stop and end  ****
        { 
         drive_ramp(0,0);
         freqout(4, 1000, 3500);                       // Speaker tone: 1 s, 3.5 kHz
         pause(500);
         freqout(4, 1000, 3500);                       // Speaker tone: 1 s, 3.5 kHz
         pause(500);
         freqout(4, 1000, 3500);                       // Speaker tone: 1 s, 3.5 kHz
         notdone=0;                                        // stop and end
        }               // end of if less than 3 inches
       }               // end of while
     }                 // end of pixyrun
      
     void findsig()                 // ** modified to use +/- dir  i.e. local sspd * dir
     {
       if(notdone == 0) return;
       int sspd = 10;              // search speed was 10  ***
       sspd = sspd * dir;
       drive_speed(sspd,-sspd);
       pause(1760);                  // turn ~60 deg one way  was 2000  ***
       adc0= adc_volts(0);
     // printi("  findsig %f\n",adc0);   // *** for debug
       if(adc0 <2)                     // Pixy pin 1 low -- no objct detected
       {
        drive_speed(-sspd,sspd);  
        pause(3520);                // turn ~120 deg the other  was 4000  ***
        adc0= adc_volts(0);
        if(adc0 <2)
        {
         drive_speed(sspd,-sspd);
         pause(920);            // turn back to center was 1000  ***
         drive_ramp(0,0);           // stop 
        }
       }
     }                     // end findsig          
      
    
  • ValeTValeT Posts: 308
    edited August 2014 Vote Up0Vote Down
    I have been able to take a better look at your code twm; thanks for posting it! Are you doing this in SimpleIDE?

    Would you mind if I borrowed snippets of your code for my project?
  • NWCCTVNWCCTV Posts: 3,629
    edited August 2014 Vote Up0Vote Down
    I am getting a build error when I just try and build with nothing connected at the end of the program:

    collect2: ld returned 1 exit statusDone. Build Failed!


    Check source for bad function call or global variable name `_printi'
    C:\Users\ANDYN~1.NWU\AppData\Local\Temp\cc131wmG.o: In function `.L39':
    (.text+0x1ca):
    } // end while
    }
    
    Andy North

    My Index Page:
  • twm47099twm47099 Posts: 553
    edited August 2014 Vote Up0Vote Down
    ValeT wrote: »
    I have been able to take a better look at your code twm; thanks for posting it! Are you doing this in SimpleIDE?

    Would you mind if I borrowed snippets of your code for my project?

    I am using SimpleIDE.
    You can use the code as you want.

    Tom
  • twm47099twm47099 Posts: 553
    edited August 2014 Vote Up0Vote Down
    NWCCTV wrote: »
    I am getting a build error when I just try and build with nothing connected at the end of the program:

    collect2: ld returned 1 exit statusDone. Build Failed!


    Check source for bad function call or global variable name `_printi'
    C:\Users\ANDYN~1.NWU\AppData\Local\Temp\cc131wmG.o: In function `.L39':
    (.text+0x1ca):
    } // end while
    }
    

    Andy,
    I'm not sure I understand what the error is. Is it the 'printi' function or the '} // end while' ?
    I just started using printi after Steve (Jazzed) told me it used less memory than 'print'. It actually saves about 5k.
    The ' } // end while ' is just the closing brace for the while(1) statement and a comment to help me keep track of braces.

    I copied the code from post #1 and pasted it into a SimpleIDE new project (first deleting all of the stuff that SimpleIDE loads into a new project), and built the program on a PC that has SimpleIDE installed (no prop board attached.) It built without any errors.

    I am using the latest version of the Learn Libraries.
    Clicking on the icon in the lower left corner of the SimpleIDE screen, I have:
    Project Options:
    Board Type: Activity Board
    Compiler: C
    Memory Model CMM Main Ram Compact
    Optimization Os-Size
    Compiler:
    Checked: 32bit doubles
    Other compiler options: -std=c99
    Linker:
    Checked: math lib

    I hope this helps.
    Tom
  • NWCCTVNWCCTV Posts: 3,629
    edited August 2014 Vote Up0Vote Down
    Do not be too concerned. I will work more with it once I connect up my EBT. The error hits right at the end of the program and is pointing at that statement. No worries right now though.
    Andy North

    My Index Page:
  • Steph LindsaySteph Lindsay Posts: 753
    edited August 2014 Vote Up0Vote Down
    This is really impressive, Tom! Thanks for posting your code.
  • edited August 2014 Vote Up0Vote Down
    Andy, if you update your Learn folder, the compiler error might go away.

    http://learn.parallax.com/propeller-c-set-simpleide/update-simpleide-workspace

    Andy
  • twm47099twm47099 Posts: 553
    edited August 2014 Vote Up0Vote Down
    I've made some modifications to the code:
    - cleaned up some code and comments

    - changed the adc function I used to the one that returns raw values - everything integer and saved about 1k memory.

    - Added a second movement control function to the Pixy code. The original (control1()) just uses a constant rotation increment - small enough to minimize overshoot. The new one (control2()) uses proportional control with larger rotation when further away from center. The control method defaults to control1(), but button 4 turns control2() on or off. The control method can be changed anytime, including when pixy is searching. The p26 and p27 LEDs light when the Pixy functions are running to show which method is being used:
    Both OFF = object out of FOV and findsig() is running, One one = control1, Both on = control2.

    - The control2() constants are set up as #define vales at the beginning of the program so the user can try different values. To get more granularity, I might change that code to floating point, but for now it is integer. The values in the program work.

    - added code at the start to send a string to the android device to set all buttons to off.

    - moved the starting 'beep' so that when it sounds, the joystick is active (no more delay after the beep).

    The latest version of the BT app has a zero button which centers the joystick, and essentially acts as an emergency stop. I'm trying to come up with a method to use the remaining buttons (once I get rid of the E-stop code) and the joystick to change the proportion control constants. The idea is to be able to change them without recompiling and loading the prop. Not sure how yet.

    The revised program follows.

    Tom
    /* C code for use with Propeller ActivityBot and Joystick BT Commander android App by Kas -  
    
     (c) T. Montemarano 2014-08-01,  MIT License
      Tested with Samsung Tablet 4
     2014-08-23  Added option for 2 pixy motion control modes (button 4), changed ADC to raw (int)
     2014-08-22  Moved Pixy movement control to functions, send initialzation string to android to clear buttons on screen
     2014-08-18 Added code for Pixy
     2014-08-13 Deleted & from function name in cog_run statement, added 'flgping' flag to show when ping is running.
     2014-08-12 Made global declaration of *cogping, increased cog stack sizes, fixed call to do_ping
     2014-08-09  used 'volatile' in global declarations.
     Written for BT Commander V5 - Changed protocol -- need to do data TX to android
     coded BT buttons: 6 = 'end' to terminate program
                                5 = E-stop  -- moving Joystick restarts movement
                                4 = Pixy movement control selection (1 or 2)
                                3 = Turn Pixy control on and off
                                2 = Turn Ping)) on and off
     Uncomment print statements and Run with terminal to debug or see command execution.
      
     App options setup
         Joystick Props -- behavior - deselect auto return to center
                                 constraint - box
         Buttons Props -- display 6
                                 labels -- 1, 2-ping, 3-pixy, 4-pixy ctrl 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 9 (will be Propeller Rx), BT DI to Prop pin 8 (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.
     abs(y) >15 = move fwd (JS positive), 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
     Ping)) has signal connected to pin 5 with 2kohm resistor in series
     common cathode rbg led has red to pin 6 with 100 ohm resistor, green to pin 7 with 120 ohm resistor, cathode to gnd.
      
      Uses Pixy interface mode 3 -- voltage corresponding to x-position of largest object
      Pixypin 1 -- abot adc0, Ppin2 -- +5v, Ppin3 -- adc1, Ppin6 -- gnd
      Color needs to be unique and large enough to see at a distance.  Laser pointer dot is too small.
      
     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.
      
     */
     #include "simpletools.h" 
     #include "fdserial.h"
     #include "abdrive.h"
     #include "ping.h"
     #include "adcDCpropab.h"  
      
     #define rspd1 3              //  speed for each wheel to rotate L or R when centering color ctrl1
     #define rspd2 10              //  speed for each wheel to rotate L or R when centering color ctrl2
     #define fspd 30              // forward speed
     #define pyp1 2048         // adc0 pin high value
     #define pylft 1045          // pixy adc raw left tolerance    was 1117
     #define pyrt 1657           // pixy rt tolerance   was 1585
     #define pyctr 1351          // pixy adc raw center
     #define pypro 3            // pixy ctrl2 proportional divisor
     #define pyoffset 0            // pixy ctrl2 offset
      
     volatile char c1[] = {0, 2, '0','0','0','0','0','0',1,'1','2','0',4,5,3};   // string to clear buttons on BT LCD
     volatile int c0 = 1;
     volatile int nbytes;   
     volatile int joydata;
     int flgping, flgpixy;               //  flags for cog, 1 = running, 0 = stopped
     int xval, yval; 
     int flgbt; 
     int dist;
     int adc0, adc1;           // adc raw
     int dir = -1;              //  -1 = left, +1 = right
     int dist1;    
     int notdone = 1;
     int pyerr, pycrct;        // pixy ctrl2 error (adc raw units from center) and correction (ms)
     int pyc2flg;               //  0 = use ctrl 1,  1 = use ctrl 2 
     int *cogbtjoy, *cogping, *cogpixy;      //  pointers to cog IDs
      
     fdserial *blut;     // declare bluetooth as serial device
     
     void getbtjoy();
     void do_ping();
     void findsig();
     void pixyrun();
     void control1();
     void control2();
     
      int main()
     {
       int x2;
       int y2;
       int xy;
       int yspd = 0;
       int xspd = 0;
       flgping = flgpixy = 0;               //    ping cog is not running, pixy not running
       pyc2flg = 0;                            // pixy will use control 1
       cogbtjoy = cog_run(getbtjoy, 220);  
       drive_ramp(0, 0);
       while(c0)
       {
         if((nbytes == 8) && (flgpixy == 0)) 
         {
           pause(50);        //   try to get rid of pause
           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) * 15;         // 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);
           drive_ramp(yspd+xspd, yspd-xspd);         // drive, turning differential added to yspd
         }
         if(nbytes == 3) 
         {
           nbytes = 0;
           printi(" button = %d\n\n", c1[2]);
           if((c1[2]=='C' )&& (flgping == 0))                  // Button 2, on turns on ping
           {
             if(flgpixy)
              { 
               cog_end(cogpixy);
               flgpixy = 0;
              }        
             cogping = cog_run(do_ping,120);  
             printi(" ping cog = %d\n\n", *cogping);
             nbytes = 0;
             flgping = 1;                      // set flgping -- ping cog is running
           }
           if(c1[2]=='D' && flgping)                // Button 2, off turns off ping, don't try to stop if not running
           {
             cog_end(cogping);
             nbytes = 0;
             flgping = 0;                      // clear flgping -- ping cog is stopped
           }
           if((c1[2]=='E') && (flgpixy ==0))                  // Button 3, on turns on pixy, turn off ping cog
           {
             if(flgping) 
             {
               cog_end(cogping);       //  if ping running when pixy turned on, turn off
               flgping = 0;
             }
             notdone = 1;
             cogpixy = cog_run(pixyrun,120);  
             printi(" pixy cog = %d\n\n", *cogpixy);
             nbytes = 0;
             flgpixy = 1;                      // set flgpixy -- pixy is running
           }
           if(c1[2]=='F' && flgpixy)                // Button 3, off turns offpixy, don't try to stop if not running
           {
             notdone = 0;
             drive_ramp(0,0); 
             cog_end(cogpixy);
             nbytes = 0;
             flgpixy = 0;                      // clear flgpixy -- pixy cog is stopped
           }
         }
       } // end while c0
       drive_ramp(0, 0);     // stop movement at end of program
       //  printi(" end \n\n");
         if(flgping) cog_end(cogping);       //  if ping running when quit is pressed, turn off
         if(flgpixy) cog_end(cogpixy);       // if pixy running when quit is pressed, turn off
         cog_end(cogbtjoy);                  // end bt
         freqout(4, 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(9, 8, 0, 115200); 
       for(int q =1; q<=14; q++)  { fdserial_txChar(blut, c1[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)
           {
             nbytes = 8;
             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) 
           {
             nbytes = 3;
             // printi(" button = %d\n\n", c1[2]);
           }
           if((i==3) && (c1[2]=='G')) pyc2flg = 1;       // set pixy control2
           if((i==3) && (c1[2]=='H')) pyc2flg = 0;       // reset pixy control1      
           if((i==3) && (c1[2]=='K' || c1[2]=='L'))      //  Button 6, ends program
           {
             if(flgpixy)
             { 
              cog_end(cogpixy); 
              flgpixy = 0;
               drive_ramp(0,0);
             }
             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
             if(flgpixy)
             { 
              cog_end(cogpixy);
              flgpixy = 0; 
             }
             drive_ramp(0, 0);
           }
         } // end if c1=2
       } // end while c0
     } // end getbtjoy
     
    
     void do_ping() 
     {
       while(1)
       {
         dist = ping_cm(5);
         if(dist<=20)            // turn on red LED
         {
           high(6);
           low(7);
         }
         else if(dist<=36)      // turn on yellow LED
         {
           high(6);
           high(7);
         }
         else                    //  turn on green LED
         {
           low(6);
           high(7);
         }
         pause(50);
       }   // end while
     }
     
    
     void pixyrun()                               // Main pixy function
     {
      drive_ramp(0,0); 
      freqout(4, 1000, 3500);                       // Speaker tone: 1 s, 3.5 kHz
      adc_init(21, 20, 19, 18);               // CS=21, SCL=20, DO=19, DI=18
      drive_setRampStep(1);               // 4 ticks per 50th of sec is default
      while(notdone)                                // Loop repeats indefinitely
       {
         adc0 = adc_in(0);
     // printi("%f\n",adc0);              // *** for debug
         if(adc0 < pyp1)                     // Pixy pin 1 = low -- no object detected
         {
          drive_ramp(0,0);   // Stop - color signature out of pixy FOV, approx +/- 37.5 deg.
          findsig();               // rotate activitybot to find color
         }
          else                     // object in field of view
           {
             if(pyc2flg) 
             { 
              control2();   
              }
             else control1();
           }         //  end of main if-else
        pause(200); 
        dist1 = ping_cm(5);                    // Get cm distance from Ping)))
        if(dist1<11)                             //  less than 11 cm -- stop and end  pixy ****
        { 
         drive_ramp(0,0);
         freqout(4, 500, 3500);                       // Speaker tone: .5 s, 3.5 kHz
         pause(500);
         freqout(4, 500, 3500);                       // Speaker tone: .5 s, 3.5 kHz
         pause(500);
         freqout(4, 500, 3500);                       // Speaker tone: .5 s, 3.5 kHz
         notdone=0;                                        // stop and end pixy
        }               // end of if less than 11 cm
       }               // end of while
     }                 // end of pixyrun
     
      
     void findsig()                 // turn abot 60, -120, 60 deg to search for color signature
     {  
          low(26);         // turn off leds P26, P27
          low(27);
       if(notdone == 0) return;
       int sspd = 19;              //  ticks for 60 degrees
       sspd = sspd * dir;
       drive_goto(sspd,-sspd);   // turn 60 deg 
         adc0 = adc_in(0);
     // printi("  findsig %f\n",adc0);   // *** for debug
       if(adc0 < pyp1)                     // Pixy pin 1 low -- no object detected
       {
         drive_goto(-sspd,sspd); 
         drive_goto(-sspd,sspd);         // turn ~120 deg the other way
         adc0 = adc_in(0);
         if(adc0 <pyp1)
        {
          drive_goto(sspd,-sspd);      // turn back to center
          drive_ramp(0,0);           // stop 
        }
       }
     }                     // end findsig          
     
      
     void control1()       //  aim ABot at object w color signature
     {
               high(26);   // turn on 1 led for ctrl 1
               low(27);
          adc1 = adc_in(1);
           if(adc1< pylft)                //  color sig to left of center
           {
             dir = -1;
             drive_speed(-rspd1,rspd1);
             pause(30);  
           }
            else if(adc1> pyrt)         //  color sig to right of center
            {
             dir = 1;
             drive_speed(rspd1,-rspd1);
             pause(30);   
            }
            else if((adc1 <= pyrt) && (adc1 >= pylft))      //  Color sig at center, play with limits 
            {
             drive_ramp(fspd,fspd);      // move incrementally to object
             pause(100);
            }
     }     //    end control1
     
      
     void control2()
     {
               high(26);         // turn on 2 leds for ctrl 2
               high(27); 
          adc1 = adc_in(1);
           if(adc1< pylft)                //  color sig to left of center
           {
             dir = -1;
             pyerr = pyctr - adc1;
             pycrct = ((pyerr * 84) / (5 * rspd2 * pypro)) +pyoffset; 
                                            //   = (number of ticks frm center / speed * prop factor)+offset
             drive_speed(-rspd2,rspd2);
             pause(pycrct);        // uses time as control variable
             drive_ramp(0,0);
           }
            else if(adc1> pyrt)         //  color sig to right of center
            {
             dir = 1;
             pyerr = adc1 - pyctr;
             pycrct = ((pyerr * 84) / (5 * rspd2 * pypro)) +pyoffset;
             drive_speed(rspd2,-rspd2);
             pause(pycrct);
             drive_ramp(0,0);   
            }
            else if((adc1 <= pyrt) && (adc1 >= pylft))      //  Color sig at center, play with limits 
            {
             drive_ramp(fspd,fspd);      // move incrementally to object
             pause(100);
            }
     }     //    end control2
     
    
    
  • twm47099twm47099 Posts: 553
    edited September 2014 Vote Up0Vote Down
    I made major change to the Pixy part of the program. Originally, I used the Pixy D to A mode that sent the 'x' value of the largest object with a defined color signature. It was the simplest method to use and it worked well. But it had a limitation in that it did not discriminate by what defined color it sent. So it was best if only 1 color was "taught" to Pixy.

    The new version uses the Pixy UART mode. This mode sends all defined signatures as a series of 2 byte values. It sends lowest byte of each word first. Pixy sends serial data as a Frame made up of Blocks of data.

    Each Block is a detected object. The format of a block is the start word 0x55, 0xAA (or AA55 when made into one 16 bitword); the next word is a checksum = the sum of the next 10 bytes. The 3rd word is the signature number. (Pixy can be taught up to 7 colors, signatures are numbered 1 to 7). The 4th word is the x value (horizontal location in the field of view); the 5th is the y value; the 6th is the horizontal size, and 7th is the vertical size.

    The start of a new Frame is an additional start word 0xAA55. So to determine the start of a frame the program has to look for the serial sequence 0x55, 0xAA, 0x55, 0xAA.

    The biggest difficulty using Pixy data is that it is continuously sending data, and every 20ms it starts a new Frame (even if part way through a Block -- it just discards the remaining data from that block). If the user program could tell Pixy to start sending a Frame, or if the Frame start word was different from the Block start word, it wold be much less complicated.

    Along with changing to use the Pixy UART, the program has added the ability to move one of 3 defined colors (signatures 1, 2, or 3). The target signature can be changed at any time (even while the Bot is chasing a color) by pressing soft button 1 on the android device. Each press cycles the number 1, 2, 3, 1, etc. The active signature number is shown in the data display in the lower left of the screen.

    Other changes are the button lights on the android screen are cleared (made dim) at the start of the program, and are updated to show what is on and what is off.

    I'm going to post the beginning comments in this post and the actual program in the following post because it is large.

    Tom
    /*   bt commander drive 10.c   This is the best version for getting x from signatures 1,2, or 3
                     New pixyUART function added.   The UART function worked.  
                     Need to consider the use of a header file.
     
    
     C code for use with Propeller ActivityBot and Joystick BT Commander android App by Kas - 
     
    
     (c) T. Montemarano 2014-08-01,  MIT License
      Tested with Samsung Tablet 4
     204-09-03   Added button 1 controls color signature - default =1, cycles to 2, then 3, then 1 etc
                       Added transmits button status and color sig number back to BT device for display.
                       Changed pixy baud to 57600, more reliable than 115200.
     2014-09-02  Added code for pixy UART function (based on good pixyuart fnctn 3.c)  Test worked.
                         Need to add BT button 1 to select signature 1 or 2.
     2014-08-23  Added option for 2 pixy motion control modes (button 4), changed ADC to raw (int)
     2014-08-22  Moved Pixy movement control to functions, send initialzation string to android to clear buttons on screen
     2014-08-18 Added code for Pixy
     2014-08-13 Deleted & from function name in cog_run statement, added 'flgping' flag to show when ping is running.
     2014-08-12 Made global declaration of *cogping, increased cog stack sizes, fixed call to do_ping
     2014-08-09  used 'volatile' in global declarations.
     Written for BT Commander V5 - Changed protocol -- need to do data TX to android
     coded BT buttons: 6 = 'end' to terminate program
                                5 = E-stop  -- moving Joystick restarts movement
                                4 = Pixy movement control selection (1 or 2)
                                3 = Turn Pixy control on and off
                                2 = Turn Ping)) on and off
                                1 = Color Signature Number cycles with each press 1, 2, 3, 1, etc
     Uncomment print statements and Run with terminal to debug or see command execution.
     
    
     App options setup
         Joystick Props -- behavior - deselect auto return to center
                                 constraint - box
         Buttons Props -- display 6
                                 labels -- 1-Color Signature, 2-ping, 3-pixy, 4-pixy ctrl 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 9 (will be Propeller Rx), BT DI to Prop pin 8 (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.
     abs(y) >15 = move fwd (JS positive), 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
     Ping)) has signal connected to pin 5 with 2kohm resistor in series
     common cathode rbg led has red to pin 6 with 100 ohm resistor, green to pin 7 with 120 ohm resistor, cathode to gnd.
     
    
      Uses Pixy interface mode 2 -- UART
      Pixypin 1 -- abot p0, 
      Ppin2      -- +5v, 
      Ppin4      -- p1, 
      Ppin6      -- gnd
      Color needs to be unique and large enough to see at a distance.  Laser pointer dot is too small.
     
    
     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.
     
    
     */
     
    
    
    
    
    
  • twm47099twm47099 Posts: 553
    edited September 2014 Vote Up0Vote Down
    Here's the actual C program;

    Tom
    #include "simpletools.h"  #include "fdserial.h"
     #include "abdrive.h"
     #include "ping.h"
      
     #define pybaud 57600    // This baud for pixy seems maximum 
     #define rspd1 3              //  speed for each wheel to rotate L or R when centering color ctrl1
     #define rspd2 10              //  speed for each wheel to rotate L or R when centering color ctrl2
     #define fspd 30              // forward speed
     #define pyp1 2048         // adc0 pin high value
     #define pylft 1045          // pixy adc raw left tolerance    was 1117
     #define pyrt 1657           // pixy rt tolerance   was 1585
     #define pyctr 1351          // pixy adc raw center
     #define pypro 3            // pixy ctrl2 proportional divisor
     #define pyoffset 0            // pixy ctrl2 offset
      
     #define pynumblks 14      // max nmbr blocks make large enough to get sig of interest, 
                                                 //  but too large gets errors
     #define blks 16         // make this pynumblks + 2
                                 
     volatile char c1[9]; 
     volatile char btstring[] = {0, 2, '0','0','0','0','0','0',1,' ',' ','1',4,5,3};
     volatile int c0 = 1;
     volatile int nbytes;   
     volatile int joydata;
     int flgping, flgpixy;               //  flags for cog, 1 = running, 0 = stopped
     int dist, dist1, flgbt, xval, yval;
     int adc0, adc1;           // was adc raw  - now used as scaled position of object
     int pyerr, pycrct;        // pixy ctrl2 error (adc raw units from center) and correction (ms)
     int pyc2flg;               //  0 = use ctrl 1,  1 = use ctrl 2 
     int pyx, pyk, pycktot, pyflg;     // pyflg = 1 when first sync word located
     int dir = -1;              //  -1 = left, +1 = right
     int notdone = 1;
     int signum = 1;         // color signature
     int pyrmax = 2703;       // maximum x value converted to raw
                         
     int pv[blks][7];                 // pv[pynmblks+2][7]
     int pyxflg =0;
     int pixyx = -1;
     int pyflg2 =1;
     int pycount = 300;     //  number of tries for good pixy read or return -1, can probably lower value
      
     int *cogbtjoy, *cogping, *cogpixy;      //  pointers to cog IDs
      
     fdserial *blut;     // declare bluetooth as serial device
     fdserial *pixy;     // declare pixy as a serial device
    
     void getbtjoy();
     void do_ping();
     void findsig();
     void pixyrun();
     void control1();
     void control2();
     int getpyx(int,int);        // getpyx(signum, pycount) 
    
     int main()
     {
       int x2;
       int y2;
       int xy;
       int yspd = 0;
       int xspd = 0;
       flgping = flgpixy = 0;               //    ping cog is not running, pixy not running
       pyc2flg = 0;                            // pixy will use control 1
       cogbtjoy = cog_run(getbtjoy, 220);  
       drive_ramp(0, 0);
       while(c0)
       {
         if((nbytes == 8) && (flgpixy == 0)) 
         {
           pause(50);        //   try to get rid of pause
           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) * 15;         // 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);
           drive_ramp(yspd+xspd, yspd-xspd);         // drive, turning differential added to yspd
         }
         if(nbytes == 3) 
         {
           nbytes = 0;
           printi(" button = %d\n\n", c1[2]);
           if((c1[2] =='A') || (c1[2] == 'B'))             // Button 1 cyles through Color Signature (1 - 3)
           {
             if(signum<3) 
             {
               signum++;
              btstring[11]=0x30+signum;
             }
             else 
             {
               signum = 1;
               btstring[11]=0x30+signum;
             }
           }
           if((c1[2]=='C' )&& (flgping == 0))                  // Button 2, on turns on ping
           {
             if(flgpixy)
              { 
               fdserial_close(pixy);       // need to close pixy fdserial port when pixy functions stopped
               cog_end(cogpixy);
               flgpixy = 0;
               drive_ramp(0,0);
               btstring[5] = 0x30;
              }        
             cogping = cog_run(do_ping,120);  
             printi(" ping cog = %d\n\n", *cogping);
             nbytes = 0;
             flgping = 1;                      // set flgping -- ping cog is running
             btstring[6] = 0x31;
           }
           if(c1[2]=='D' && flgping)                // Button 2, off turns off ping, don't try to stop if not running
           {
             cog_end(cogping);
             nbytes = 0;
             flgping = 0;                      // clear flgping -- ping cog is stopped
             btstring[6] = 0x30;
           }
           if((c1[2]=='E') && (flgpixy ==0))                  // Button 3, on turns on pixy, turn off ping cog
           {
             if(flgping) 
             {
               cog_end(cogping);       //  if ping running when pixy turned on, turn off
               flgping = 0;
             btstring[6] = 0x30;
             }
             notdone = 1;
             cogpixy = cog_run(pixyrun,120);  
             printi(" pixy cog = %d\n\n", *cogpixy);
             nbytes = 0;
             flgpixy = 1;                      // set flgpixy -- pixy is running
             btstring[5] = 0x31;
           }
           if(c1[2]=='F' && flgpixy)                // Button 3, off turns offpixy, don't try to stop if not running
           {
             notdone = 0;
             drive_ramp(0,0); 
             fdserial_close(pixy);
             cog_end(cogpixy);
             nbytes = 0;
             flgpixy = 0;                      // clear flgpixy -- pixy cog is stopped
             btstring[5] = 0x30;
           }
       for(int q =1; q<=14; q++)  { fdserial_txChar(blut, btstring[q]) ; }   // send string to BT
         }
       } // end while c0
       drive_ramp(0, 0);     // stop movement at end of program
       //  printi(" end \n\n");
         if(flgping) cog_end(cogping);       //  if ping running when quit is pressed, turn off
         if(flgpixy) cog_end(cogpixy);       // if pixy running when quit is pressed, turn off
         cog_end(cogbtjoy);                  // end bt
         freqout(4, 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(9, 8, 0, 115200); 
       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)
           {
             nbytes = 8;
             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) 
           {
             nbytes = 3;
             // printi(" button = %d\n\n", c1[2]);
           }
           if((i==3) && (c1[2]=='G')) 
           {
             pyc2flg = 1;       // set pixy control2
             btstring[4] = 0x31;
           }
           if((i==3) && (c1[2]=='H')) 
           {
             pyc2flg = 0;       // reset pixy control1
             btstring[4] = 0x30;
           } 
           if((i==3) && (c1[2]=='K' || c1[2]=='L'))      //  Button 6, ends program
           {
             if(flgpixy)
             { 
              fdserial_close(pixy);
              cog_end(cogpixy); 
              flgpixy = 0;
               drive_ramp(0,0);
             }
             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
             if(flgpixy)
             {  
              fdserial_close(pixy);
              cog_end(cogpixy);
              flgpixy = 0;           
             }
             drive_ramp(0, 0);
           }
         } // end if c1=2
       } // end while c0
     } // end getbtjoy
      
    
     void do_ping() 
     {
       while(1)
       {
         dist = ping_cm(5);
         if(dist<=20)            // turn on red LED
         {
           high(6);
           low(7);
         }
         else if(dist<=36)      // turn on yellow LED
         {
           high(6);
           high(7);
         }
         else                    //  turn on green LED
         {
           low(6);
           high(7);
         }
         pause(50);
       }   // end while
     }
     
    
     void pixyrun()                               // Main pixy function
     {
          pixy = fdserial_open(1, 0, 0, pybaud);   // For ActivityBot
      drive_ramp(0,0);
      pause(50); 
      freqout(4, 1000, 3500);                       // Speaker tone: 1 s, 3.5 kHz
      drive_setRampStep(1);               // 4 ticks per 50th of sec is default
      while(notdone)                                // Loop repeats indefinitely
       {
         pyx = getpyx(signum, pycount);
     // printi("x = %d\n", pyx);              // *** for debug 
         if(pyx == -1)                     // pyx =-1 -- no object detected
         {
          drive_ramp(0,0);   // Stop - color signature out of pixy FOV, approx +/- 37.5 deg.
          findsig();               // rotate activitybot to find color
         }
          else                     // object in field of view
           {
             adc1 = (pyrmax  * pyx) / 300;    // convert to adc raw values  
             if(pyc2flg) 
             { 
              control2();   
              }
             else control1();
           }         //  end of main if-else
        pause(200); 
        dist1 = ping_cm(5);                    // Get cm distance from Ping)))
        if(dist1<11)                             //  less than 11 cm -- stop and end  pixy ****
        { 
         drive_ramp(0,0);
         freqout(4, 500, 3500);                       // Speaker tone: .5 s, 3.5 kHz
         pause(500);
         freqout(4, 500, 3500);                       // Speaker tone: .5 s, 3.5 kHz
         pause(500);
         freqout(4, 500, 3500);                       // Speaker tone: .5 s, 3.5 kHz
         notdone=0;                                        // stop and end pixy
        }               // end of if less than 11 cm
     pause(100);
       }               // end of while
     }                 // end of pixyrun
      
    
     void findsig()                 // turn abot 60, -120, 60 deg to search for color signature
     {  
          low(26);         // turn off leds P26, P27
          low(27);
       if(notdone == 0) return;
       int sspd = 19;              //  ticks for 60 degrees
       sspd = sspd * dir;
       drive_goto(sspd,-sspd);   // turn 60 deg 
       pyx = getpyx(signum, pycount);
       if(pyx == -1)                     // pyx =-1 -- no object detected
       {
         drive_goto(-sspd,sspd); 
         drive_goto(-sspd,sspd);         // turn ~120 deg the other way
         pyx = getpyx(signum, pycount);
         if(pyx == -1)                     // pyx =-1 -- no object detected
        {
          drive_goto(sspd,-sspd);      // turn back to center
          drive_ramp(0,0);           // stop 
        }
       }
     }                     // end findsig          
      
    
     void control1()       //  aim ABot at object w color signature
     {
               high(26);   // turn on 1 led for ctrl 1
               low(27);
     //     adc1 = adc_in(1);
           if(adc1< pylft)                //  color sig to left of center
           {
             dir = -1;
             drive_speed(-rspd1,rspd1);
             pause(30);  
           }
            else if(adc1> pyrt)         //  color sig to right of center
            {
             dir = 1;
             drive_speed(rspd1,-rspd1);
             pause(30);   
            }
            else if((adc1 <= pyrt) && (adc1 >= pylft))      //  Color sig at center, play with limits 
            {
             drive_ramp(fspd,fspd);      // move incrementally to object
             pause(100);
            }
     }     //    end control1
     
    
     void control2()
     {
               high(26);         // turn on 2 leds for ctrl 2
               high(27); 
     //     adc1 = adc_in(1);
           if(adc1< pylft)                //  color sig to left of center
           {
             dir = -1;
             pyerr = pyctr - adc1;
             pycrct = ((pyerr * 84) / (5 * rspd2 * pypro)) +pyoffset; 
                                            //   = (number of ticks frm center / speed * prop factor)+offset
             drive_speed(-rspd2,rspd2);
             pause(pycrct);        // uses time as control variable
             drive_ramp(0,0);
           }
            else if(adc1> pyrt)         //  color sig to right of center
            {
             dir = 1;
             pyerr = adc1 - pyctr;
             pycrct = ((pyerr * 84) / (5 * rspd2 * pypro)) +pyoffset;
             drive_speed(rspd2,-rspd2);
             pause(pycrct);
             drive_ramp(0,0);   
            }
            else if((adc1 <= pyrt) && (adc1 >= pylft))      //  Color sig at center, play with limits 
            {
             drive_ramp(fspd,fspd);      // move incrementally to object
             pause(100);
            }
     }     //    end control2
      
    
     int getpyx(int z, int cntr)           //      z is the signature number 1 - 7  cntr is pycount
     {
     char c1;                     // raw LS byte values from pixy
     int i;
     int j;                                //  number of pixy words
     int k;
     pixyx = -1;
     pyflg2 = 1;
     pause(50);
       while(pyflg2)
      {
         cntr--;
         pyflg = 0;
         fdserial_rxFlush(pixy);
         c1 = fdserial_rxChar(pixy);
         if(c1 == 0x55)        // 1st
         {
           pv[0][0] = fdserial_rxChar(pixy);
           if(pv[0][0] == 0xAA)     //  one sync word    2nd
           {
             pyflg =1;
             c1 = fdserial_rxChar(pixy);
             if(c1== 0x55)   // 3rd
             {
               pv[0][0] = fdserial_rxChar(pixy);
               if(pv[0][0] == 0xAA) // 2 sync words = start of frame   4th 
               {
                 pv[0][0] = 0xAA55;
                 pyflg =2; 
                 for(i = 1; i < 7; i++)   
                 {
                   c1  = fdserial_rxChar(pixy);
                   pv[0][i] = (fdserial_rxChar(pixy) << 8) | c1;
                 }
                 k = 1;
                 for(j = 1; j < pynumblks; j++)  //  aa
                 {
                   for(i = 0; i < 7; i++)  //  bb
                   {
                     c1  = fdserial_rxChar(pixy);
                     pv[j][i] = (fdserial_rxChar(pixy) << 8) | c1;
                     if((pv[j][1] != 0xAA55) && (pv[j][2] != 0)) k= j+1;       //  2 sync words or empty block 
                    }   // end bb
                 }  // end aa
     //           printi(" \n----- new frame -----\n");
                for(i=0; i < k; i++)       //  cc  k = num good blocks
                {
                  pycktot = 0;
                  for(j=2; j<7; j++) pycktot += pv[i][j];      // calc chksum
                 
                  if((pv[i][1] !=0) && pycktot == pv[i][1])      // if cksum is good and chksum<>0  5th
                  {
                     pyk = k;
                     if((pv[i][2] == z) && (pyxflg == 0))        //  dd
                     {
                       pyxflg = 1;
                       pixyx = pv[i][3];
                       pyflg2 =0;
                     }     // end dd
                  }  // end 5th
                }  //  end cc
                pyxflg = 0;
                return pixyx;
               }   // end 4th
             } // end 3rd
           }  // end 2nd
         }  // end 1st
         if(cntr<0)
         {
           pyflg2 =0;
           return pixyx;
         }
      }  // end while
     }  // end function
     
    
    
  • NWCCTVNWCCTV Posts: 3,629
    edited September 2014 Vote Up0Vote Down
    What pins are Rx and Tx connected to?
    Andy North

    My Index Page:
  • twm47099twm47099 Posts: 553
    edited September 2014 Vote Up0Vote Down
    NWCCTV wrote: »
    What pins are Rx and Tx connected to?

    I assume you are asking about the blue tooth to Activity board (not the Pixy to Activity Board). The Sparkfun RN 42 module uses the designations DO and DI. It also defaults to 115200 baud.

    "Uses RN42 Blutooth module. I'm using Sparkfun part that has same form as X-Bee socket. Connect Bluetooth DO to Activity Board pin 9 (will be Propeller Rx), Bluetooth DI to Prop pin 8 (will be Prop Tx)"

    I use this command to start the communication:
                   
    blut = fdserial_open(9, 8, 0, 115200);    // fdserial * fdserial_open(int rxpin, int txpin, int mode, int baudrate) 
    

    If you want to use different pins, just change the values in: fdserial_open(9, 8, 0, 115200);

    Hope this helps

    Tom
  • NWCCTVNWCCTV Posts: 3,629
    edited September 2014 Vote Up0Vote Down
    Yes, this helps. Only problem is I just found out my EBT is dead.
    Andy North

    My Index Page:
  • NWCCTVNWCCTV Posts: 3,629
    edited September 2014 Vote Up0Vote Down
    Next question. Will this work with the cheapy eBay BT modules?
    Andy North

    My Index Page:
  • JonnyMacJonnyMac Posts: 5,739
    edited January 2015 Vote Up0Vote Down
    Next question. Will this work with the cheapy eBay BT modules?

    Mine did. I wrote my code in Spin which can be found in this thread:
    -- http://forums.parallax.com/showthread.php/159920-Android-Control-of-Propeller-using-Joystick-BT-Commander-App-Spin-Code

    Note that my code is not driving an ActivityBot yet -- will do that while watching the Super Bowl. As I like to take things in steps, I wrote program that has the core functionality to deal with JBTC. For the time being, it reports data to the screen, sends data back, and controls the LEDs on the PAB using buttons from JBTC. I think it proves that everything works
    Jon McPhalen
      *It's "Jon" or "JonnyMac" -- please don't call me "Jonny"
  • edited February 2015 Vote Up0Vote Down
    Hey Tom,

    Your app from Post #1 is really nice! I just showed some of our techs and education staff, and they all really liked it.

    Sheesh, I can't believe this has been gathering dust tucked away in a forum post for more than a year. Mind if we publish an ActivityBot project with a paired down version of the code? Attribution to your forum handle or elsewhere? Please send me a PM with any specifics.

    For anyone using the RN-42 Bluetooth module, make sure to enable Auto Discovery with a jumper over position-2 (2nd from right). The other three configuration settings should have no jumpers.

    Andy
  • twm47099twm47099 Posts: 553
    edited February 2015 Vote Up0Vote Down
    Hey Tom,

    Your app from Post #1 is really nice! I just showed some of our techs and education staff, and they all really liked it.

    Sheesh, I can't believe this has been gathering dust tucked away in a forum post for more than a year. Mind if we publish an ActivityBot project with a paired down version of the code? Attribution to your forum handle or elsewhere? Please send me a PM with any specifics.

    For anyone using the RN-42 Bluetooth module, make sure to enable Auto Discovery with a jumper over position-2 (2nd from right). The other three configuration settings should have no jumpers.

    Andy

    Andy,
    I just want to clarify, the C code is mine, and you can use it as you like. One of the things I was doing with that code was learning how to start, stop, and restart cogs.

    The author of the android app is 'Kas'; he has an account on these forums and did some projects with the Basic Stamp. Since then he is an arduino user and has a long thread regarding his app at:

    http://forum.arduino.cc/index.php?PHPSESSID=oshm7d4o43o9vh1f3f70jo7l76&topic=173246.285

    He gave me a lot of help to understand his code. I'm sure he would be very happy to have more exposure for his app.

    The detailed information on the data formats sent from the android to the micro-controller and from the micro to the android is shown in his post here:

    http://forum.arduino.cc/index.php?topic=173246.msg1766491#msg1766491

    Note that in posts 14 and 15 of this thread, I added code to send data (button status and data) from the Propeller to the android device. It sets the buttons to lit or grey and lists simple data on the android device. You might want to add code like that to a later segment of your project.

    (In posts 14-15 the major complication is the code to get data that controls the ActivityBot from the CMUCAM5 (Pixy) color tracking device. I've used Prop to Pixy interfacing via the ActivityBot ADC and using a UART connection, I'm slowly working on code to use Pixy's SPI interface with the SPI PASM library you helped me develop. -- the problem with that is the Pixy developers keep updating their protocols -- Removing the pixy functions simplifies the code a lot.)

    Tom
  • edited March 2015 Vote Up0Vote Down
    Thank you Tom for all the info! I will use and cite the resources accordingly. Thank you also to Wildatheart who broke the trail for us with the EZ-Bluetooth in C & a nice App thread.
  • Congratulations , I have tested your program and android app Joy BT Comander with Bluetooth RN42XV pis similar Xbee Module and it Whorks very fine in my ActivityBot.
    I would like gige to you Thanks for all your Effors to do this wonderful project i have used it .
    I appressiate you shows it in Parallax Forums.
    Thanks Very Much. my Respects an my aknowlements to you.
    my best regards.

    Eng. Jesus Perez Carreño (Mexico)
    P.D. Very sorry for my poor English
  • Welcome to the forums, jesuspca! I am so glad to hear your Android-app controlled ActivityBot project is working. We do have wonderful forum members who are very generous to share their projects. I hope you keep posting. Your English is much better than my Spanish!
Sign In or Register to comment.