Secret Spyno: How to add a new agent

February 21, 2015 by Aaron

Your Spyno Agents do all the work, so you don’t have to. Train agents for each website you want to gather information from. You tell each agent what information to grab, as well as how to present the information to you. Here’s how…

Visit

chrome://extensions

1

chrome://extensions

in Chrome’s address bar
Open the options page
You’ll see this Spyno options page
Scroll to the bottom and click in the box for “Add New Agent”

Agent example (options explained below):

{
  "name": "reddit",
  "schedule": {
    "periodInMinutes": 15
  },
  "url": "http://www.reddit.com/hot.json?limit=50",
  "extract": {
    "root": "$.data.children[*]",
    "fields": {
      "title": "$.data.title",
      "permalink": "$.data.permalink",
      "url": "$.data.url"
    },
    "template": "<div><h3>reddit</h3></div>{{#each rows}}<div><a href=\"{{url}}\">{{title}}</a> <a href=\"http://reddit.com{{permalink}}\">&raquo;</a></div>{{/each}}"
  }
}

{

"name": "reddit",

"schedule": {

"periodInMinutes": 15

"url": "http://www.reddit.com/hot.json?limit=50",

"extract": {

"root": "$.data.children[*]",

"fields": {

"title": "$.data.title",

"permalink": "$.data.permalink",

"url": "$.data.url"

"template": "<div><h3>reddit</h3></div>{{#each rows}}<div><a href=\"{{url}}\">{{title}}</a> <a href=\"http://reddit.com{{permalink}}\">»</a></div>{{/each}}"

}

Line 2 name: unique name for your Spyno agent (required)
Line 4 periodInMinutes: how often you would like Spyno to fetch from the source URL
Line 6 url: the source URL that has the info you want Spyno to fetch
Line 7 extract: this section tells Spyno what info to grab and how you want it presented
Line 8 root: JSONPath to the list of records you’re interested in
Line 9 fields: JSONPath to the fields you’re interested in, and what name you want to give them
Line 14 template: Handlebars template to tell Spyno how to draw the info you fetched. Spyno automatically packages your info into a list of “rows”, so your template should always have

{{#each rows}}{{/each}}

1	{{#each rows}}{{/each}}

You may include any html in your template, including ‹style›. E.g.,

"template": "
<style>
.fancy {
  font-variant: small-caps;
}
</style> 
<h3>reddit</h3>
{{#each rows}}
  <div class='fancy'><a href=\"{{url}}\">{{title}}</a> 
  <a href=\"http://reddit.com{{permalink}}\">&raquo;</a></div>
{{/each}}
"

"template": "

<style>

.fancy {

font-variant: small-caps;

}

</style>

<h3>reddit</h3>

<div class='fancy'><a href=\"{{url}}\">{{title}}</a>

Next: How to design a layout for your Spyno page

Arduino Vader “Force Grip” remote car (a.k.a. smartphone virtual wheel)

January 22, 2015 by Aaron

The 3-wheel car is built with the Makeblock Robot Starter Kit. (Plus some Legos for style.) It has a Bluetooth module. It is controlled by the phone’s gyros via Bluetooth. You drive it like a steering wheel with two hands, or like Darth Vader with one hand.

1. Code for the smartphone below. I saved this as gyroCar.py and transferred it to my phone (LG Optimus Dynamic). Then I execute/run it using SL4A from the phone itself:

#!/usr/bin/env python

import android
import sys
import time

class RoboComm:
  def __init__(self, droid, baotooth):
    self.droid = droid
    self.baotooth = baotooth
    self.lastCommand = None

  def sendCmd(self, cmd):
    if cmd is None:
      return 

    if "fwd" in cmd or "bwd" in cmd \
      or "left" in cmd or "right" in cmd \
      or "stop" in cmd:
      # don't send redundant movement commands to save data
      if self.lastCommand == cmd:
        return

    print cmd
    self.lastCommand = cmd

    if self.baotooth:
      self.droid.bluetoothWrite(cmd + "\n", self.baotooth)

class DroidGyroJoystick:
  def __init__(self, droid):
    self.droid = droid
    self.orient = None
    self.joyFwdBwdPos = 0
    self.joyLeftRightPos = 0
    self.accelZero = 0.4 # deflection of less than this is treated as no deflection
    self.turnZero = 0.3 # deflection of less than this is treated as no deflection
    self.minSpeed = 0.4

    if self.droid:
      self.droid.startSensingTimed(1, 200)

    self.updateOrient()

  def updateOrient(self):
    if self.droid:
      self.orient = self.droid.sensorsReadOrientation().result

  def getJoyCommand(self):
    #
    # translate gyro reading to joystick input
    #
    if not self.orient:
      return None

    if self.orient[0] is None:
      return None

    turn = self.orient[1] # [1.57=left, 0=stop, -1.57=right]
    accel = self.orient[2] # [0=fwd, -1.57=stop, -3.14=bwd] 

    #if accel > 0 and accel < 0.7:
    #  # tipping phone too far forward but let's treat as fwd anyway
    #  accel = 0

    if turn > 1.57:
      turn = 1.57 # hard left, past horizon = max left
    if turn < -1.57:
      turn = -1.57 # hard right, past horizon = max right

    if accel > 0:
      # phone is inverted - send stop signal
      return "!stop"

    # normalize turn and accel values to [-1.0, 1.0]
    turn = (-turn) / 1.57
    accel = ((-accel) - 1.57) / 1.57

    # filter for neutral pos
    if turn > -self.turnZero and turn < self.turnZero:
      turn = 0
    if accel > -self.accelZero and accel < self.accelZero:
      accel = 0

    # set min speed
    if turn > 0:
      turn = max(self.minSpeed, turn)
    if turn < 0:
      turn = min(-self.minSpeed, turn)
    if accel > 0:
      accel = max(self.minSpeed, accel)
    if accel < 0:
      accel = min(-self.minSpeed, accel)

    self.joyFwdBwdPos = int(accel * 10)
    self.joyLeftRightPos = int(turn * 10)

    # decide move cmd based on vectors
    fbmag = min(abs(self.joyFwdBwdPos) + 1, 10)
    lrmag = min(abs(self.joyLeftRightPos) + 2, 10)

    if self.joyFwdBwdPos < 0:
      if self.joyLeftRightPos < 0:
        return "!fwdleft %d %d" % (fbmag, lrmag)
      elif self.joyLeftRightPos > 0:
        return "!fwdright %d %d" % (fbmag, lrmag)
      else:
        return "!fwd %d" % fbmag
    elif self.joyFwdBwdPos > 0:
      if self.joyLeftRightPos < 0:
        return "!bwdleft %d %d" % (fbmag, lrmag)
      elif self.joyLeftRightPos > 0:
        return "!bwdright %d %d" % (fbmag, lrmag)
      else:
        return "!bwd %d" % fbmag
    elif self.joyLeftRightPos  < 0:
      return "!left %d" % lrmag
    elif self.joyLeftRightPos > 0:
      return "!right %d" % lrmag

    return "!stop"

class GyroCar:
  def __init__(self):
    self.droid = None
    self.baotooth = None

    self.droid = android.Android()
    #self.droid = android.Android(('192.168.1.6', 33333))
    self.droid.wakeLockAcquireDim()
    #self.droid.generateDtmfTones('123')
    self.connectBluetooth()

    self.roboComm = RoboComm(self.droid, self.baotooth)
    self.gyro = DroidGyroJoystick(self.droid)

  def __del__(self):
    print "Goodbye!"
    if self.droid:
      if self.baotooth:
        self.droid.bluetoothStop(self.baotooth)
      self.droid.wakeLockRelease()

  def connectBluetooth(self):
    uuid = "00001101-0000-1000-8000-00805F9B34FB"
    mac = "98:D3:31:B0:E9:37" # makeblock mac

    self.droid.bluetoothConnect(uuid, mac)
    conns = self.droid.bluetoothActiveConnections()
    if not conns.result:
      print "Android phone failed to link to Baotooth"
    else:
      print "Baotooth connected"
      self.baotooth = conns.result.keys()[0]

    if (self.baotooth):
      self.droid.bluetoothWrite("!beep\n", self.baotooth)

  def run(self):
    # poll gyro, translate to bao commands
    # run forever
    while True:
      self.gyro.updateOrient()
      cmd = self.gyro.getJoyCommand()
      self.roboComm.sendCmd(cmd)
      time.sleep(0.2)

# allow use as a module or standalone script
if __name__ == "__main__":
  gyroCar = GyroCar()
  gyroCar.run()

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

#!/usr/bin/env python

import android

import sys

import time

class RoboComm:

def __init__(self, droid, baotooth):

self.droid = droid

self.baotooth = baotooth

self.lastCommand = None

def sendCmd(self, cmd):

if cmd is None:

return

if "fwd" in cmd or "bwd" in cmd \

or "left" in cmd or "right" in cmd \

or "stop" in cmd:

# don't send redundant movement commands to save data

if self.lastCommand == cmd:

return

print cmd

self.lastCommand = cmd

if self.baotooth:

self.droid.bluetoothWrite(cmd + "\n", self.baotooth)

class DroidGyroJoystick:

def __init__(self, droid):

self.droid = droid

self.orient = None

self.joyFwdBwdPos = 0

self.joyLeftRightPos = 0

self.accelZero = 0.4 # deflection of less than this is treated as no deflection

self.turnZero = 0.3 # deflection of less than this is treated as no deflection

self.minSpeed = 0.4

if self.droid:

self.droid.startSensingTimed(1, 200)

self.updateOrient()

def updateOrient(self):

if self.droid:

self.orient = self.droid.sensorsReadOrientation().result

def getJoyCommand(self):

# translate gyro reading to joystick input

if not self.orient:

return None

if self.orient[0] is None:

return None

turn = self.orient[1] # [1.57=left, 0=stop, -1.57=right]

accel = self.orient[2] # [0=fwd, -1.57=stop, -3.14=bwd]

#if accel > 0 and accel < 0.7:

# # tipping phone too far forward but let's treat as fwd anyway

# accel = 0

if turn > 1.57:

turn = 1.57 # hard left, past horizon = max left

if turn < -1.57:

turn = -1.57 # hard right, past horizon = max right

if accel > 0:

# phone is inverted - send stop signal

return "!stop"

# normalize turn and accel values to [-1.0, 1.0]

turn = (-turn) / 1.57

accel = ((-accel) - 1.57) / 1.57

# filter for neutral pos

if turn > -self.turnZero and turn < self.turnZero:

turn = 0

if accel > -self.accelZero and accel < self.accelZero:

accel = 0

# set min speed

if turn > 0:

turn = max(self.minSpeed, turn)

if turn < 0:

turn = min(-self.minSpeed, turn)

if accel > 0:

accel = max(self.minSpeed, accel)

if accel < 0:

accel = min(-self.minSpeed, accel)

self.joyFwdBwdPos = int(accel * 10)

self.joyLeftRightPos = int(turn * 10)

# decide move cmd based on vectors

fbmag = min(abs(self.joyFwdBwdPos) + 1, 10)

lrmag = min(abs(self.joyLeftRightPos) + 2, 10)

if self.joyFwdBwdPos < 0:

if self.joyLeftRightPos < 0:

return "!fwdleft %d %d" % (fbmag, lrmag)

elif self.joyLeftRightPos > 0:

return "!fwdright %d %d" % (fbmag, lrmag)

else:

return "!fwd %d" % fbmag

elif self.joyFwdBwdPos > 0:

if self.joyLeftRightPos < 0:

return "!bwdleft %d %d" % (fbmag, lrmag)

elif self.joyLeftRightPos > 0:

return "!bwdright %d %d" % (fbmag, lrmag)

else:

return "!bwd %d" % fbmag

elif self.joyLeftRightPos < 0:

return "!left %d" % lrmag

elif self.joyLeftRightPos > 0:

return "!right %d" % lrmag

return "!stop"

class GyroCar:

def __init__(self):

self.droid = None

self.baotooth = None

self.droid = android.Android()

#self.droid = android.Android(('192.168.1.6', 33333))

self.droid.wakeLockAcquireDim()

#self.droid.generateDtmfTones('123')

self.connectBluetooth()

self.roboComm = RoboComm(self.droid, self.baotooth)

self.gyro = DroidGyroJoystick(self.droid)

def __del__(self):

print "Goodbye!"

if self.droid:

if self.baotooth:

self.droid.bluetoothStop(self.baotooth)

self.droid.wakeLockRelease()

def connectBluetooth(self):

uuid = "00001101-0000-1000-8000-00805F9B34FB"

mac = "98:D3:31:B0:E9:37" # makeblock mac

self.droid.bluetoothConnect(uuid, mac)

conns = self.droid.bluetoothActiveConnections()

if not conns.result:

print "Android phone failed to link to Baotooth"

else:

print "Baotooth connected"

self.baotooth = conns.result.keys()[0]

if (self.baotooth):

self.droid.bluetoothWrite("!beep\n", self.baotooth)

def run(self):

# poll gyro, translate to bao commands

# run forever

while True:

self.gyro.updateOrient()

cmd = self.gyro.getJoyCommand()

self.roboComm.sendCmd(cmd)

time.sleep(0.2)

# allow use as a module or standalone script

if __name__ == "__main__":

gyroCar = GyroCar()

gyroCar.run()

If you use the code above, the part that you’ll need to tweak for your own build is the DroidGyroJoystick class. Particularly, the pitch/roll numbers my phone gives lying on one edge may be different from yours. The other things you might want to change are the tuning parameters. For example, I configured a “deadzone” with accelZero and turnZero so that small motions near “centered” position won’t count as joystick inputs.

(If you’re new to SL4A, I show how to get set up, as well as where the android.py in “import android” above comes from, here.)

2. Arduino sketch for the robot car below. Sorry it’s unnecessarily complicated. It’s because it’s code for a 2-in-1 IR or Bluetooth + obstacle-avoidance car. If you connect only an IR sensor to Port 6, it will be IR-controlled. If you connect only a Bluetooth module to Port 7, it will be Bluetooth-controlled. Why did I do such a thing? Because I wanted to be able to switch remotes for my kids just by swapping modules, without uploading sketches.

Anyway, what’s important here is that in Bluetooth mode, it recognizes my silly custom command set (fwd, left, right, bwd, etc). These command strings are how the SL4A Python script on the phone instructs the car what to do, over Bluetooth. The other useful thing is that the Bluetooth commands let me specify the speed, in addition to direction. So the more you push your joystick forward, the faster it’ll go.

#include <Makeblock.h>
#include <Arduino.h>
#include <String.h>
#include <SoftwareSerial.h>
#include <Wire.h>

MeDCMotor MotorL(M1);  
MeDCMotor MotorR(M2);
MeUltrasonicSensor ultrasonic(PORT_3);
MeInfraredReceiver infrared(PORT_6);
MeBluetooth bluetooth(PORT_7);

const int MAXBUFSZ = 128;
char g_lineBuf[MAXBUFSZ]; 
int g_bufReadIdx = 0;

// movement settings
const boolean g_debug = false;
const int g_stepPeriod = 50;
const int g_maxStepsForward = 3000 / g_stepPeriod;
const int g_maxTurnSteps = 3000 / g_stepPeriod;
const int g_minSpeed = 45;
const int g_maxSpeed = 255;
const int g_speedFactor = 23;
const float g_leftTrim = 1.;
const float g_rightTrim = 1.;

// enums
const int LED_PIN = 13;
const int EBRAKE_STOP = 2; // full stop
const int EBRAKE_WAIT = 1; // wait for "Go" signal
const int EBRAKE_GO = 0; // brakes off and GO!
const int MODE_ULTRASONIC = 0;
const int MODE_IR = 1;
const int MODE_BLUETOOTH = 2;
const int MODE_UNKNOWN = 3;
const int ULTRASONIC_INIT_SPEED = 200;

// robo state machine
int g_moveSpeed = ULTRASONIC_INIT_SPEED;
int g_numStepsForward = 0;
int g_numTurnSteps = 0;
boolean g_rightFlag;
int g_ebrake = EBRAKE_STOP;
long g_timeLastBlocked = 0;
uint8_t g_mode = MODE_UNKNOWN;
uint8_t g_lastMode = MODE_UNKNOWN;

void setup()
{
  Serial.begin(9600);
  Serial.println("Software serial on");

  bluetooth.begin(9600);
  Serial.println("Bluetooth on");

  infrared.begin(); 
  Serial.println("Infrared on");

  g_lineBuf[0] = '\0';
  ResetUltrasonic(EBRAKE_STOP);
  pinMode(LED_PIN, OUTPUT); // "obstructed" LED

  Serial.println("Bao ready");
}

void loop()
{
  boolean hasBlue = false;
  boolean hasRed = false;

  if (g_mode == MODE_UNKNOWN)
  {
    // first time - detect if we're in IR or Bluetooth mode
    hasRed = infrared.available() || infrared.buttonState();
    hasBlue = bluetooth.available();

    if (hasRed)
      SwitchMode(MODE_IR);
    if (hasBlue)
      SwitchMode(MODE_BLUETOOTH);
  }
  else if (g_mode == MODE_IR || g_lastMode == MODE_IR)
    hasRed = infrared.available() || infrared.buttonState();
  else if (g_mode == MODE_BLUETOOTH || g_lastMode == MODE_BLUETOOTH)
    hasBlue = bluetooth.available();

  char inDat;
  if (hasBlue)
  {
    ResetUltrasonic(EBRAKE_GO);
    inDat = bluetooth.read();
    Serial.print(inDat);

    readIntoLineBuf(inDat);
    if (cmdLineReady())
    {
      String cmd(g_lineBuf);
      if (g_debug)
        Serial.println("COMMAND " + cmd);
      processCommand(cmd);
    }
  }
  else if (hasRed)
  {
    ResetUltrasonic(EBRAKE_GO);
    inDat = infrared.read();
    switch (inDat)
    {
      case IR_BUTTON_PLUS: 
        Forward(g_moveSpeed);
        break;
      case IR_BUTTON_MINUS:
        Backward(g_moveSpeed);
        break;
      case IR_BUTTON_NEXT:
        TurnRight(g_moveSpeed*2);
        break;
      case IR_BUTTON_PREVIOUS:
        TurnLeft(g_moveSpeed*2);
        break;
      case IR_BUTTON_TEST:
        ForwardAndLeft(g_moveSpeed, g_moveSpeed);
        break;
      case IR_BUTTON_RETURN:
        ForwardAndRight(g_moveSpeed, g_moveSpeed);
        break;
      case IR_BUTTON_9:
        ChangeSpeed(g_speedFactor*9+g_minSpeed);
        break;
      case IR_BUTTON_8:
        ChangeSpeed(g_speedFactor*8+g_minSpeed);
        break;
      case IR_BUTTON_7:
        ChangeSpeed(g_speedFactor*7+g_minSpeed);
        break;
      case IR_BUTTON_6:
        ChangeSpeed(g_speedFactor*6+g_minSpeed);
        break;
      case IR_BUTTON_5:
        ChangeSpeed(g_speedFactor*5+g_minSpeed);
        break;
      case IR_BUTTON_4:
        ChangeSpeed(g_speedFactor*4+g_minSpeed);
        break;
      case IR_BUTTON_3:
        ChangeSpeed(g_speedFactor*3+g_minSpeed);
        break;
      case IR_BUTTON_2:
        ChangeSpeed(g_speedFactor*2+g_minSpeed);
        break;
      case IR_BUTTON_1:
        ChangeSpeed(g_speedFactor*1+g_minSpeed);
        break;
      case IR_BUTTON_POWER:
        buzzerOn();
        delay(100);
        buzzerOff();
        break;
      case IR_BUTTON_MENU:
        if (g_mode != MODE_ULTRASONIC)
        {
          // go ultrasonic
          buzzerOn();
          delay(100);
          buzzerOff();
          delay(100);
          buzzerOn();
          delay(100);
          buzzerOff();
          delay(100);
          buzzerOn();
          delay(300);
          buzzerOff();

          SwitchMode(MODE_ULTRASONIC);
          g_moveSpeed = ULTRASONIC_INIT_SPEED;
        }
        else
        {
          // back to manual remote
          buzzerOn();
          delay(100);
          buzzerOff();
          digitalWrite(LED_PIN, LOW);

          SwitchMode(MODE_IR);
        }

        break;
      default:
        break;
    }
  }
  else
  {
    if (g_mode == MODE_ULTRASONIC
      && millis() % g_stepPeriod == 0)
      doUltrasonicCar();
    else if (g_mode == MODE_IR)
      Stop();
  }
}

bool processCommand(String cmd)
{
  if (cmd == "!beep")
  {
    buzzerOn();
    delay(100);
    buzzerOff();
  }
  else if (cmd.startsWith("!fwd "))
    Forward(getSpeed(cmd, 0));
  else if (cmd.startsWith("!bwd "))
    Backward(getSpeed(cmd, 0));
  else if (cmd.startsWith("!left "))
    TurnLeft(getSpeed(cmd, 0));
  else if (cmd.startsWith("!right "))
    TurnRight(getSpeed(cmd, 0));
  else if (cmd.startsWith("!fwdleft"))
    ForwardAndLeft(getSpeed(cmd, 0), getSpeed(cmd, 1));
  else if (cmd.startsWith("!fwdright"))
    ForwardAndRight(getSpeed(cmd, 0), getSpeed(cmd, 1));
  else if (cmd.startsWith("!bwdleft"))
    BackwardAndTurnLeft(getSpeed(cmd, 0), getSpeed(cmd, 1));
  else if (cmd.startsWith("!bwdright"))
    BackwardAndTurnRight(getSpeed(cmd, 0), getSpeed(cmd, 1));
  else
    Stop();
}

int getSpeed(String cmd, int idx)
{
  int mag = getCmdInt(cmd, idx);
  return g_speedFactor * mag + g_minSpeed;
}

int getCmdInt(String cmd, int idx)
{
  int spaceIdx = cmd.indexOf(' ');
  for (int i=0; i < idx; i++)
    spaceIdx = cmd.indexOf(' ', spaceIdx+1);

  return cmd.substring(spaceIdx+1).toInt();
}

bool cmdLineReady()
{
  if (g_bufReadIdx != 0)
    return false; // still building line

  if (g_lineBuf[0] != '!')
    return false; // not a command line

  return true;
}

void readIntoLineBuf(char c)
{
  if (c == '\n' || c == '\r' || c == '\0')
  {
    // finish line for processing, set ready for next line
    g_bufReadIdx = 0;
    return;
  }

  if (g_bufReadIdx >= MAXBUFSZ - 1)
  {
    // drop overflow bytes
    return;
  }

  g_lineBuf[g_bufReadIdx] = c;
  g_lineBuf[g_bufReadIdx+1] = '\0';
  g_bufReadIdx++;
}

void doUltrasonicCar()
{
  int distance = ultrasonic.distanceCm();
  
  boolean isBlocked = (distance > 0 && distance < 60);
  if (isBlocked)
    g_timeLastBlocked = millis();
  else if (millis() - g_timeLastBlocked < 100)
    isBlocked = true;

  if (isBlocked)
    digitalWrite(LED_PIN, HIGH);
  else
    digitalWrite(LED_PIN, LOW);

  boolean isStuck 
    = (g_numTurnSteps > g_maxTurnSteps
      || g_numStepsForward > g_maxStepsForward);

  if (g_ebrake == EBRAKE_STOP && !isBlocked)
  {
    // /unstuck, ready for go signal
    g_ebrake = EBRAKE_WAIT; 
    return;
  }
  else if (g_ebrake == EBRAKE_WAIT && isBlocked)
  {
    // "Go!"
    ResetUltrasonic(EBRAKE_GO);
    return;
  }
  else if (g_ebrake == EBRAKE_GO && isStuck)
  {
    // stuck -- yank ebrake
    g_ebrake = EBRAKE_STOP;
    Stop();
    return;
  }

  if (g_ebrake != EBRAKE_GO)
  {
    // halt until !blocked and receive "Go" signal
    return;
  }
  
  if (isBlocked)
  {
    // evasive maneuvers!
    randomSeed(analogRead(A4));
    int randnum = random(300);

    if (distance > 30)
    {
      if (randnum > 150 && !g_rightFlag)
        TurnLeft(g_moveSpeed);   
      else
        TurnRight(g_moveSpeed);  
    }
    else
    {
      if (randnum > 150) 
        BackwardAndTurnLeft(g_moveSpeed, g_moveSpeed);
      else 
        BackwardAndTurnRight(g_moveSpeed, g_moveSpeed);
    }
  }
  else
  {
    // ONWARD!
    Forward(g_moveSpeed);
  }
}

void ResetUltrasonic(int brakePos)
{
  g_rightFlag = false;
  g_numStepsForward = 0;
  g_numTurnSteps = 0;
  g_ebrake = brakePos;
  
  if (g_debug && g_mode == MODE_ULTRASONIC)
    Serial.println("Reset");
}

void Stop()
{
  MotorL.stop();
  MotorR.stop();
}

void Forward(int speed)
{
  g_numStepsForward++;
  g_numTurnSteps = 0;
  g_rightFlag = false;

  MotorL.run(speed * g_leftTrim);
  MotorR.run(speed * g_rightTrim);
  
  if (g_debug)
    Serial.println("Forward");
}

void ForwardAndLeft(int fspeed, int lspeed)
{
  g_numStepsForward++;
  g_numTurnSteps++;
  g_rightFlag = false;

  MotorL.run(fspeed - (lspeed / 2));
  MotorR.run(fspeed);
  
  if (g_debug)
    Serial.println("Forward+Left");
}

void ForwardAndRight(int fspeed, int rspeed)
{
  g_numStepsForward++;
  g_numTurnSteps++;
  g_rightFlag = true;

  MotorL.run(fspeed);
  MotorR.run(fspeed - (rspeed / 2));
  
  if (g_debug)
    Serial.println("Forward+Right");
}

void Backward(int speed)
{
  g_rightFlag = false;
  g_numTurnSteps = 0;
  MotorL.run(-speed);
  MotorR.run(-speed);

  if (g_debug)
    Serial.println("Backward");
}

void TurnLeft(int speed)
{
  g_rightFlag = false;
  g_numStepsForward = 0;
  g_numTurnSteps++;

  MotorL.run(-speed / 2);
  MotorR.run(speed / 2);
  
  if (g_debug)
    Serial.println("Left");
}

void TurnRight(int speed)
{
  g_rightFlag = true;
  g_numStepsForward = 0;
  g_numTurnSteps++;

  MotorL.run(speed / 2);
  MotorR.run(-speed / 2);
  
  if (g_debug)
    Serial.println("Right");
}

void BackwardAndTurnLeft(int bspeed, int lspeed)
{
  g_rightFlag = true;
  g_numStepsForward = 0;
  g_numTurnSteps++;

  MotorL.run(-bspeed + (lspeed / 2));
  MotorR.run(-bspeed);

  if (g_debug)
    Serial.println("Backward+Left");
}

void BackwardAndTurnRight(int bspeed, int rspeed)
{
  g_rightFlag = false;
  g_numStepsForward = 0;
  g_numTurnSteps++;

  MotorL.run(-bspeed);
  MotorR.run(-bspeed + (rspeed / 2));

  if (g_debug)
    Serial.println("Backward+Right");
}

void SwitchMode(uint8_t newMode)
{
  g_lastMode = g_mode;
  g_mode = newMode;
}

void ChangeSpeed(int spd)
{
  g_moveSpeed = spd;
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

#include <Makeblock.h>

#include <Arduino.h>

#include <String.h>

#include <SoftwareSerial.h>

#include <Wire.h>

MeDCMotor MotorL(M1);

MeDCMotor MotorR(M2);

MeUltrasonicSensor ultrasonic(PORT_3);

MeInfraredReceiver infrared(PORT_6);

MeBluetooth bluetooth(PORT_7);

const int MAXBUFSZ = 128;

char g_lineBuf[MAXBUFSZ];

int g_bufReadIdx = 0;

// movement settings

const boolean g_debug = false;

const int g_stepPeriod = 50;

const int g_maxStepsForward = 3000 / g_stepPeriod;

const int g_maxTurnSteps = 3000 / g_stepPeriod;

const int g_minSpeed = 45;

const int g_maxSpeed = 255;

const int g_speedFactor = 23;

const float g_leftTrim = 1.;

const float g_rightTrim = 1.;

// enums

const int LED_PIN = 13;

const int EBRAKE_STOP = 2; // full stop

const int EBRAKE_WAIT = 1; // wait for "Go" signal

const int EBRAKE_GO = 0; // brakes off and GO!

const int MODE_ULTRASONIC = 0;

const int MODE_IR = 1;

const int MODE_BLUETOOTH = 2;

const int MODE_UNKNOWN = 3;

const int ULTRASONIC_INIT_SPEED = 200;

// robo state machine

int g_moveSpeed = ULTRASONIC_INIT_SPEED;

int g_numStepsForward = 0;

int g_numTurnSteps = 0;

boolean g_rightFlag;

int g_ebrake = EBRAKE_STOP;

long g_timeLastBlocked = 0;

uint8_t g_mode = MODE_UNKNOWN;

uint8_t g_lastMode = MODE_UNKNOWN;

void setup()

{

Serial.begin(9600);

Serial.println("Software serial on");

bluetooth.begin(9600);

Serial.println("Bluetooth on");

infrared.begin();

Serial.println("Infrared on");

g_lineBuf[0] = '\0';

ResetUltrasonic(EBRAKE_STOP);

pinMode(LED_PIN, OUTPUT); // "obstructed" LED

Serial.println("Bao ready");

}

void loop()

{

boolean hasBlue = false;

boolean hasRed = false;

if (g_mode == MODE_UNKNOWN)

{

// first time - detect if we're in IR or Bluetooth mode

hasRed = infrared.available() || infrared.buttonState();

hasBlue = bluetooth.available();

if (hasRed)

SwitchMode(MODE_IR);

if (hasBlue)

SwitchMode(MODE_BLUETOOTH);

}

else if (g_mode == MODE_IR || g_lastMode == MODE_IR)

hasRed = infrared.available() || infrared.buttonState();

else if (g_mode == MODE_BLUETOOTH || g_lastMode == MODE_BLUETOOTH)

hasBlue = bluetooth.available();

char inDat;

if (hasBlue)

{

ResetUltrasonic(EBRAKE_GO);

inDat = bluetooth.read();

Serial.print(inDat);

readIntoLineBuf(inDat);

if (cmdLineReady())

{

String cmd(g_lineBuf);

if (g_debug)

Serial.println("COMMAND " + cmd);

processCommand(cmd);

}

else if (hasRed)

{

ResetUltrasonic(EBRAKE_GO);

inDat = infrared.read();

switch (inDat)

{

case IR_BUTTON_PLUS:

Forward(g_moveSpeed);

break;

case IR_BUTTON_MINUS:

Backward(g_moveSpeed);

break;

case IR_BUTTON_NEXT:

TurnRight(g_moveSpeed*2);

break;

case IR_BUTTON_PREVIOUS:

TurnLeft(g_moveSpeed*2);

break;

case IR_BUTTON_TEST:

ForwardAndLeft(g_moveSpeed, g_moveSpeed);

break;

case IR_BUTTON_RETURN:

ForwardAndRight(g_moveSpeed, g_moveSpeed);

break;

case IR_BUTTON_9:

ChangeSpeed(g_speedFactor*9+g_minSpeed);

break;

case IR_BUTTON_8:

ChangeSpeed(g_speedFactor*8+g_minSpeed);

break;

case IR_BUTTON_7:

ChangeSpeed(g_speedFactor*7+g_minSpeed);

break;

case IR_BUTTON_6:

ChangeSpeed(g_speedFactor*6+g_minSpeed);

break;

case IR_BUTTON_5:

ChangeSpeed(g_speedFactor*5+g_minSpeed);

break;

case IR_BUTTON_4:

ChangeSpeed(g_speedFactor*4+g_minSpeed);

break;

case IR_BUTTON_3:

ChangeSpeed(g_speedFactor*3+g_minSpeed);

break;

case IR_BUTTON_2:

ChangeSpeed(g_speedFactor*2+g_minSpeed);

break;

case IR_BUTTON_1:

ChangeSpeed(g_speedFactor*1+g_minSpeed);

break;

case IR_BUTTON_POWER:

buzzerOn();

delay(100);

buzzerOff();

break;

case IR_BUTTON_MENU:

if (g_mode != MODE_ULTRASONIC)

{

// go ultrasonic

buzzerOn();

delay(100);

buzzerOff();

delay(100);

buzzerOn();

delay(100);

buzzerOff();

delay(100);

buzzerOn();

delay(300);

buzzerOff();

SwitchMode(MODE_ULTRASONIC);

g_moveSpeed = ULTRASONIC_INIT_SPEED;

}

else

{

// back to manual remote

buzzerOn();

delay(100);

buzzerOff();

digitalWrite(LED_PIN, LOW);

SwitchMode(MODE_IR);

}

break;

default:

break;

}

else

{

if (g_mode == MODE_ULTRASONIC

&& millis() % g_stepPeriod == 0)

doUltrasonicCar();

else if (g_mode == MODE_IR)

Stop();

}

bool processCommand(String cmd)

{

if (cmd == "!beep")

{

buzzerOn();

delay(100);

buzzerOff();

}

else if (cmd.startsWith("!fwd "))

Forward(getSpeed(cmd, 0));

else if (cmd.startsWith("!bwd "))

Backward(getSpeed(cmd, 0));

else if (cmd.startsWith("!left "))

TurnLeft(getSpeed(cmd, 0));

else if (cmd.startsWith("!right "))

TurnRight(getSpeed(cmd, 0));

else if (cmd.startsWith("!fwdleft"))

ForwardAndLeft(getSpeed(cmd, 0), getSpeed(cmd, 1));

else if (cmd.startsWith("!fwdright"))

ForwardAndRight(getSpeed(cmd, 0), getSpeed(cmd, 1));

else if (cmd.startsWith("!bwdleft"))

BackwardAndTurnLeft(getSpeed(cmd, 0), getSpeed(cmd, 1));

else if (cmd.startsWith("!bwdright"))

BackwardAndTurnRight(getSpeed(cmd, 0), getSpeed(cmd, 1));

else

Stop();

}

int getSpeed(String cmd, int idx)

{

int mag = getCmdInt(cmd, idx);

return g_speedFactor * mag + g_minSpeed;

}

int getCmdInt(String cmd, int idx)

{

int spaceIdx = cmd.indexOf(' ');

for (int i=0; i < idx; i++)

spaceIdx = cmd.indexOf(' ', spaceIdx+1);

return cmd.substring(spaceIdx+1).toInt();

}

bool cmdLineReady()

{

if (g_bufReadIdx != 0)

return false; // still building line

if (g_lineBuf[0] != '!')

return false; // not a command line

return true;

}

void readIntoLineBuf(char c)

{

if (c == '\n' || c == '\r' || c == '\0')

{

// finish line for processing, set ready for next line

g_bufReadIdx = 0;

return;

}

if (g_bufReadIdx >= MAXBUFSZ - 1)

{

// drop overflow bytes

return;

}

g_lineBuf[g_bufReadIdx] = c;

g_lineBuf[g_bufReadIdx+1] = '\0';

g_bufReadIdx++;

}

void doUltrasonicCar()

{

int distance = ultrasonic.distanceCm();

boolean isBlocked = (distance > 0 && distance < 60);

if (isBlocked)

g_timeLastBlocked = millis();

else if (millis() - g_timeLastBlocked < 100)

isBlocked = true;

if (isBlocked)

digitalWrite(LED_PIN, HIGH);

else

digitalWrite(LED_PIN, LOW);

boolean isStuck

= (g_numTurnSteps > g_maxTurnSteps

|| g_numStepsForward > g_maxStepsForward);

if (g_ebrake == EBRAKE_STOP && !isBlocked)

{

// /unstuck, ready for go signal

g_ebrake = EBRAKE_WAIT;

return;

}

else if (g_ebrake == EBRAKE_WAIT && isBlocked)

{

// "Go!"

ResetUltrasonic(EBRAKE_GO);

return;

}

else if (g_ebrake == EBRAKE_GO && isStuck)

{

// stuck -- yank ebrake

g_ebrake = EBRAKE_STOP;

Stop();

return;

}

if (g_ebrake != EBRAKE_GO)

{

// halt until !blocked and receive "Go" signal

return;

}

if (isBlocked)

{

// evasive maneuvers!

randomSeed(analogRead(A4));

int randnum = random(300);

if (distance > 30)

{

if (randnum > 150 && !g_rightFlag)

TurnLeft(g_moveSpeed);

else

TurnRight(g_moveSpeed);

}

else

{

if (randnum > 150)

BackwardAndTurnLeft(g_moveSpeed, g_moveSpeed);

else

BackwardAndTurnRight(g_moveSpeed, g_moveSpeed);

}

else

{

// ONWARD!

Forward(g_moveSpeed);

}

void ResetUltrasonic(int brakePos)

{

g_rightFlag = false;

g_numStepsForward = 0;

g_numTurnSteps = 0;

g_ebrake = brakePos;

if (g_debug && g_mode == MODE_ULTRASONIC)

Serial.println("Reset");

}

void Stop()

{

MotorL.stop();

MotorR.stop();

}

void Forward(int speed)

{

g_numStepsForward++;

g_numTurnSteps = 0;

g_rightFlag = false;

MotorL.run(speed * g_leftTrim);

MotorR.run(speed * g_rightTrim);

if (g_debug)

Serial.println("Forward");

}

void ForwardAndLeft(int fspeed, int lspeed)

{

g_numStepsForward++;

g_numTurnSteps++;

g_rightFlag = false;

MotorL.run(fspeed - (lspeed / 2));

MotorR.run(fspeed);

if (g_debug)

Serial.println("Forward+Left");

}

void ForwardAndRight(int fspeed, int rspeed)

{

g_numStepsForward++;

g_numTurnSteps++;

g_rightFlag = true;

MotorL.run(fspeed);

MotorR.run(fspeed - (rspeed / 2));

if (g_debug)

Serial.println("Forward+Right");

}

void Backward(int speed)

{

g_rightFlag = false;

g_numTurnSteps = 0;

MotorL.run(-speed);

MotorR.run(-speed);

if (g_debug)

Serial.println("Backward");

}

void TurnLeft(int speed)

{

g_rightFlag = false;

g_numStepsForward = 0;

g_numTurnSteps++;

MotorL.run(-speed / 2);

MotorR.run(speed / 2);

if (g_debug)

Serial.println("Left");

}

void TurnRight(int speed)

{

g_rightFlag = true;

g_numStepsForward = 0;

g_numTurnSteps++;

MotorL.run(speed / 2);

MotorR.run(-speed / 2);

if (g_debug)

Serial.println("Right");

}

void BackwardAndTurnLeft(int bspeed, int lspeed)

{

g_rightFlag = true;

g_numStepsForward = 0;

g_numTurnSteps++;

MotorL.run(-bspeed + (lspeed / 2));

MotorR.run(-bspeed);

if (g_debug)

Serial.println("Backward+Left");

}

void BackwardAndTurnRight(int bspeed, int rspeed)

{

g_rightFlag = false;

g_numStepsForward = 0;

g_numTurnSteps++;

MotorL.run(-bspeed);

MotorR.run(-bspeed + (rspeed / 2));

if (g_debug)

Serial.println("Backward+Right");

}

void SwitchMode(uint8_t newMode)

{

g_lastMode = g_mode;

g_mode = newMode;

}

void ChangeSpeed(int spd)

{

g_moveSpeed = spd;

}

P.S. I just installed the WordPress “Crayon Syntax Highlighter” plugin. It prettifies blocks of code within ‹pre› tags, like above. I like it so far.

Teaser: Makeblock Robot Starter Kit Cellbot + Sensors + shxtty Path Tracking

December 12, 2014 by Aaron

Well this was fun. Ingredients:

Makeblock Bluetooth module
To execute instructions from smartphone.
LG Optimus Dynamic
Strapped on robot for sensors. WiFi to talk with Mission Control. Bluetooth to control robot.
Tornado server
Runs on PC. Connects to phone via SL4A. Talks with web UI using websockets.
Websockets
To stream sensor data from phone to web UI. Also relays commands from web UI to robot.
Web UI
Basic webpage with Javascript to handle keyboard events and draw things
D3.js
For rendering the accelerometer G-meter (not shown in video)
HTML Canvas
I just used a 2-D grid and rectangles (fillRect) to draw the robot’s snail trail.
Physics
v = d/t; d = v*t
Estimate distance traveled by robot using motor “velocity” and time.
Ye Olde Trigonometry
To convert bearing + distance traveled to x-y position in room.

You know, this could make a good school project. Lots of applied basic science, math and programming.

I promise to put up details and code in a later post 🙂

(If I forget, please email aaron@secretsciencelab.com and I’ll share everything with you.)

How to fix SL4A webCamFacade exception

December 12, 2014 by Aaron

On some phones, if you try starting the webcam using SL4A, you will get the following exception:

java.lang.RuntimeException: setParameters failed

Yuck!

Thankfully, the fix is simple. I found Paola Garcia‘s solution everywhere I looked on the internet. She was cheerfully answering everyone’s questions all over the place. She said she fixed the problem by changing the preview format from JPEG to NV21:

Thanks Paola!

But while the fix is simple, the hard part is you have to build a new SL4A app (APK) with the fix. Good news! It’s not really that hard because someone was kind enough to leave instructions. It was a good learning experience for me.

First, follow the first 2/3rds of this post:
http://jokar-johnk.blogspot.com.es/2011/02/how-to-make-android-app-with-sl4a.html
Do only the “compile” steps. The last “how to package your app” part doesn’t really apply to you, since you just want to build SL4A.

I’m writing this post because I ran into some problems using the instructions above. I’m sharing my solutions in case anyone else runs into them too. I think most of the problems are because the post was written in 2011, and the latest libraries and SDKs in 2014 don’t play as nicely together.

The step you’ll want to substitute is when you Import your project source code. Instead of the “official” SL4A, Use hg to clone the SL4A repository from Paola Garcia‘s branch:

hg clone https://olapaola@code.google.com/r/olapaola-android-scripting/

(FYI: I used Tortoise Hg on Windows.)

Now here are the problems I ran into and how I fixed them. (Just in case anyone else runs into them too.)

Missing Android SDK files when compiling
SL4A needs APIs from the older Android SDK platforms. Check C:\Users\yourname\AppData\Local\Android\sdk\platforms. If you’re missing the directories android-3, android-4, android-7, android-8, you need to download/install them.

In Eclipse: Window -> Android SDK Manager. Select all the Android SDKs til API Level 8 (Android 2.2).

Download & install
conversion to dalvik format failed with error 1
I got this when trying to export the APK after compiling. I couldn’t figure out what the problem was for a while because stupid Eclipse just gave me the error message with no console logs. I finally found the problem when I did “Project -> Run As…” It gave me a helpful error message I could debug. Something about the JDK libraries was conflicting. I made two changes:

Download JDK 1.6.0_26 and select it here

Drop the Java compiler to 1.6
Export APK
I’m an Android noob so this step tripped me up too.

Export a signed package. Create a new keystore. I filled in blanks with B.S. and set passwords to “android”.

Finally, out pops your fresh APK with WebCamFacade fixed. Transfer the APK to your phone, uninstall the old SL4A app and install this new one.

Happy hacking! If these instructions were helpful, leave a comment below. Let us know what silly things you used your camera for. I know I have my eye on OpenCV 🙂

How to build a Makeblock Bluetooth/WiFi Cellbot car

November 22, 2014 by Aaron

Ingredients:
– LG Optimus Dynamic
– PC game controller (joystick)
– pygame for joystick input
– python script runs on PC to translate joystick input to movement commands
– SL4A to send movement commands from PC to smartphone over WiFi
– smartphone sends movement commands to Arduino via Bluetooth (also SL4A)
– Arduino translates commands to movement

First, we taught Bao to listen with Bluetooth

That was using pyserial, connecting directly from PC to Arduino via COM17.

I used that to build a basic instruction set to control movement. E.g.,:
!fwd 10
!bwd 5
!left 3
!right 10
!fwdleft 10 5
!bwdright 6 6
!stop

Those instructions are parsed and executed on the Arduino. The numbers tell how much the joystick was deflected.

But it wasn’t very interesting to connect directly from PC to robot via Bluetooth. I wanted to strap my smartphone on to add that extra layer of sensors and computing power. So my scheme was for my PC to talk to my smartphone and for the smartphone to be like the Jewel Wasp, controlling the robot via Bluetooth.

This was the main Python code running on my PC:


#!/usr/bin/env python

import android
import serial
import pygame
import sys

def initAndroid():
  # LG dynamic
  global g_droid
  global g_bao

  uuid = "00001101-0000-1000-8000-00805F9B34FB"
  mac = "98:D3:31:B0:E9:37" # makeblock mac
  g_droid = android.Android(('192.168.1.3', 33333))
  g_droid.wakeLockAcquirePartial()
  g_droid.bluetoothConnect(uuid, mac)
  conns = g_droid.bluetoothActiveConnections()
  if not conns.result:
     print "Android phone failed to link to robo's Bluetooth"
     sys.exit(0)
  else:
    print "Bluetooth connected"
  g_bao = conns.result.keys()[0]
  g_droid.bluetoothWrite("!beep\n", g_bao)

g_bao = None
g_droid = None
#g_bao = serial.Serial("COM17", 9600) # laptop bluetooth
initAndroid()

g_joyFwdBwdPos = 0
g_joyLeftRightPos = 0

# allow multiple joysticks
joy = []

def cleanupAndroid():
  global g_droid 
  global g_bao

  if g_droid:
    g_droid.bluetoothStop(g_bao)
    g_droid.wakeLockRelease()

# handle joystick event
def handleJoyEvent(e):
    global g_joyFwdBwdPos
    global g_joyLeftRightPos

    if e.type == pygame.JOYAXISMOTION:
        axis = "unknown"
        if (e.dict['axis'] == 1):
            axis = "X"

        if (e.dict['axis'] == 0):
            axis = "Y"

        if (e.dict['axis'] == 2):
            axis = "Throttle"

        if (e.dict['axis'] == 3):
            axis = "Z"

        if (axis != "unknown"):
          str = "Axis: %s; Value: %f" % (axis, e.dict['value'])
          # uncomment to debug
          #output(str, e.dict['joy'])

          # Arduino joystick-servo hack

          pos = e.dict['value']
          zero = 0.5

          vector = int(pos * 10)

          # update vectors
          if axis == "X" or axis == "Z":
            if pos < -zero or pos > zero:
              g_joyFwdBwdPos = vector
            else:
              g_joyFwdBwdPos = 0

          if axis == "Y" or axis == "Throttle":
            if pos < -zero or pos > zero:
              g_joyLeftRightPos = vector
            else:
              g_joyLeftRightPos = 0

          # decide move cmd based on vectors
          fbmag = abs(g_joyFwdBwdPos)
          lrmag = abs(g_joyLeftRightPos)
          if g_joyFwdBwdPos < 0:
            if g_joyLeftRightPos < 0:
              roboCmd("!fwdleft %d %d" % (fbmag, lrmag))
            elif g_joyLeftRightPos > 0:
              roboCmd("!fwdright %d %d" % (fbmag, lrmag))
            else:
              roboCmd("!fwd %d" % fbmag) 
          elif g_joyFwdBwdPos > 0:
            if g_joyLeftRightPos < 0:
              roboCmd("!bwdleft %d %d" % (fbmag, lrmag))
            elif g_joyLeftRightPos > 0:
              roboCmd("!bwdright %d %d" % (fbmag, lrmag))
            else:
              roboCmd("!bwd %d" % fbmag)
          elif g_joyLeftRightPos  < 0:
            roboCmd("!left %d" % lrmag)
          elif g_joyLeftRightPos > 0:
            roboCmd("!right %d" % lrmag)
          else:
            roboCmd("!stop")

    elif e.type == pygame.JOYBUTTONDOWN:
        str = "Button: %d" % (e.dict['button'])
        # uncomment to debug
        output(str, e.dict['joy'])

        button = e.dict['button']

        # Button 0 (trigger) to quit
        if (button == 0):
            print "Bye!\n"
            cleanupAndroid()
            quit()
        elif (button == 1):
          roboCmd("!beep")
    else:
        pass

# print the joystick position
def output(line, stick):
    print "Joystick: %d; %s" % (stick, line)

# wait for joystick input
def joystickControl():
    while True:
        e = pygame.event.wait()
        if (e.type == pygame.JOYAXISMOTION or e.type == pygame.JOYBUTTONDOWN):
            handleJoyEvent(e)

g_lastRoboCmd = None
def roboCmd(roboCmd):
  global g_lastRoboCmd
  global g_bao

  if "fwd" in roboCmd or "bwd" in roboCmd \
    or "left" in roboCmd or "right" in roboCmd \
    or "stop" in roboCmd:
    # don't send redundant movement commands to save data
    if g_lastRoboCmd == roboCmd:
      return

  g_lastRoboCmd = roboCmd
  print roboCmd

  if g_droid:
    g_droid.bluetoothWrite(roboCmd + "\n", g_bao)
  else:
    g_bao.write(roboCmd + "\n")

# main method
def main():
    # initialize pygame
    pygame.joystick.init()
    pygame.display.init()
    if not pygame.joystick.get_count():
        print "\nPlease connect a joystick and run again.\n"
        quit()
    print "\n%d joystick(s) detected." % pygame.joystick.get_count()
    for i in range(pygame.joystick.get_count()):
        myjoy = pygame.joystick.Joystick(i)
        myjoy.init()
        joy.append(myjoy)
        print "Joystick %d: " % (i) + joy[i].get_name()
    print "Depress trigger (button 0) to quit.\n"

    # run joystick listener loop
    joystickControl()

# allow use as a module or standalone script
if __name__ == "__main__":
    main()

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

#!/usr/bin/env python

import android

import serial

import pygame

import sys

def initAndroid():

# LG dynamic

global g_droid

global g_bao

uuid = "00001101-0000-1000-8000-00805F9B34FB"

mac = "98:D3:31:B0:E9:37" # makeblock mac

g_droid = android.Android(('192.168.1.3', 33333))

g_droid.wakeLockAcquirePartial()

g_droid.bluetoothConnect(uuid, mac)

conns = g_droid.bluetoothActiveConnections()

if not conns.result:

print "Android phone failed to link to robo's Bluetooth"

sys.exit(0)

else:

print "Bluetooth connected"

g_bao = conns.result.keys()[0]

g_droid.bluetoothWrite("!beep\n", g_bao)

g_bao = None

g_droid = None

#g_bao = serial.Serial("COM17", 9600) # laptop bluetooth

initAndroid()

g_joyFwdBwdPos = 0

g_joyLeftRightPos = 0

# allow multiple joysticks

joy = []

def cleanupAndroid():

global g_droid

global g_bao

if g_droid:

g_droid.bluetoothStop(g_bao)

g_droid.wakeLockRelease()

# handle joystick event

def handleJoyEvent(e):

global g_joyFwdBwdPos

global g_joyLeftRightPos

if e.type == pygame.JOYAXISMOTION:

axis = "unknown"

if (e.dict['axis'] == 1):

axis = "X"

if (e.dict['axis'] == 0):

axis = "Y"

if (e.dict['axis'] == 2):

axis = "Throttle"

if (e.dict['axis'] == 3):

axis = "Z"

if (axis != "unknown"):

str = "Axis: %s; Value: %f" % (axis, e.dict['value'])

# uncomment to debug

#output(str, e.dict['joy'])

# Arduino joystick-servo hack

pos = e.dict['value']

zero = 0.5

vector = int(pos * 10)

# update vectors

if axis == "X" or axis == "Z":

if pos < -zero or pos > zero:

g_joyFwdBwdPos = vector

else:

g_joyFwdBwdPos = 0

if axis == "Y" or axis == "Throttle":

if pos < -zero or pos > zero:

g_joyLeftRightPos = vector

else:

g_joyLeftRightPos = 0

# decide move cmd based on vectors

fbmag = abs(g_joyFwdBwdPos)

lrmag = abs(g_joyLeftRightPos)

if g_joyFwdBwdPos < 0:

if g_joyLeftRightPos < 0:

roboCmd("!fwdleft %d %d" % (fbmag, lrmag))

elif g_joyLeftRightPos > 0:

roboCmd("!fwdright %d %d" % (fbmag, lrmag))

else:

roboCmd("!fwd %d" % fbmag)

elif g_joyFwdBwdPos > 0:

if g_joyLeftRightPos < 0:

roboCmd("!bwdleft %d %d" % (fbmag, lrmag))

elif g_joyLeftRightPos > 0:

roboCmd("!bwdright %d %d" % (fbmag, lrmag))

else:

roboCmd("!bwd %d" % fbmag)

elif g_joyLeftRightPos < 0:

roboCmd("!left %d" % lrmag)

elif g_joyLeftRightPos > 0:

roboCmd("!right %d" % lrmag)

else:

roboCmd("!stop")

elif e.type == pygame.JOYBUTTONDOWN:

str = "Button: %d" % (e.dict['button'])

# uncomment to debug

output(str, e.dict['joy'])

button = e.dict['button']

# Button 0 (trigger) to quit

if (button == 0):

print "Bye!\n"

cleanupAndroid()

quit()

elif (button == 1):

roboCmd("!beep")

else:

pass

# print the joystick position

def output(line, stick):

print "Joystick: %d; %s" % (stick, line)

# wait for joystick input

def joystickControl():

while True:

e = pygame.event.wait()

if (e.type == pygame.JOYAXISMOTION or e.type == pygame.JOYBUTTONDOWN):

handleJoyEvent(e)

g_lastRoboCmd = None

def roboCmd(roboCmd):

global g_lastRoboCmd

global g_bao

if "fwd" in roboCmd or "bwd" in roboCmd \

or "left" in roboCmd or "right" in roboCmd \

or "stop" in roboCmd:

# don't send redundant movement commands to save data

if g_lastRoboCmd == roboCmd:

return

g_lastRoboCmd = roboCmd

print roboCmd

if g_droid:

g_droid.bluetoothWrite(roboCmd + "\n", g_bao)

else:

g_bao.write(roboCmd + "\n")

# main method

def main():

# initialize pygame

pygame.joystick.init()

pygame.display.init()

if not pygame.joystick.get_count():

print "\nPlease connect a joystick and run again.\n"

quit()

print "\n%d joystick(s) detected." % pygame.joystick.get_count()

for i in range(pygame.joystick.get_count()):

myjoy = pygame.joystick.Joystick(i)

myjoy.init()

joy.append(myjoy)

print "Joystick %d: " % (i) + joy[i].get_name()

print "Depress trigger (button 0) to quit.\n"

# run joystick listener loop

joystickControl()

# allow use as a module or standalone script

if __name__ == "__main__":

main()

And this was what I had on the Arduino:



		
		
			
			

			
			#include <Makeblock.h>
#include <Arduino.h>
#include <String.h>
#include <SoftwareSerial.h>
#include <Wire.h>

MeDCMotor MotorL(M1);  
MeDCMotor MotorR(M2);
MeUltrasonicSensor ultrasonic(PORT_3);

MeBluetooth bluetooth(PORT_5);

const int MAXBUFSZ = 128;
char g_lineBuf[MAXBUFSZ]; 
int g_bufReadIdx = 0;

// movement settings
const boolean g_debug = true;
const int g_stepPeriod = 50;
const int g_maxStepsForward = 3000 / g_stepPeriod;
const int g_maxTurnSteps = 3000 / g_stepPeriod;
const int g_minSpeed = 45;
const int g_maxSpeed = 255;
const int g_speedFactor = 23;
const float g_leftTrim = .97;
const float g_rightTrim = 1.;

// enums
const int LED_PIN = 13;
const int EBRAKE_STOP = 2; // full stop
const int EBRAKE_WAIT = 1; // wait for "Go" signal
const int EBRAKE_GO = 0; // brakes off and GO!
const int MODE_ULTRASONIC = 0;
const int MODE_REMOTE = 1;
const int ULTRASONIC_INIT_SPEED = 200;

// robo state machine
int g_moveSpeed = ULTRASONIC_INIT_SPEED;
int g_numStepsForward = 0;
int g_numTurnSteps = 0;
boolean g_rightFlag;
int g_ebrake = EBRAKE_STOP;
long g_timeLastBlocked = 0;
uint8_t g_mode = MODE_REMOTE;

void setup()
{
  Serial.begin(9600);
  Serial.println("Software serial on");

  bluetooth.begin(9600);
  Serial.println("Bluetooth on");

  g_lineBuf[0] = '\0';
  ResetUltrasonic(EBRAKE_STOP);
  pinMode(LED_PIN, OUTPUT); // "obstructed" LED

  Serial.println("Bao ready");
}

void loop()
{
  char inDat;
  boolean hasBlue = false;

  hasBlue = bluetooth.available();

  if (hasBlue)
  {
    ResetUltrasonic(EBRAKE_GO);
    inDat = bluetooth.read();
    Serial.print(inDat);

    readIntoLineBuf(inDat);
    if (cmdLineReady())
    {
      String cmd(g_lineBuf);
      if (g_debug)
        Serial.println("COMMAND " + cmd);
      processCommand(cmd);
    }
  }
  else if (g_mode != MODE_REMOTE
    && millis() % g_stepPeriod == 0)
    doUltrasonicCar();
}

bool processCommand(String cmd)
{
  if (cmd == "!beep")
  {
    buzzerOn();
    delay(100);
    buzzerOff();
  }
  else if (cmd.startsWith("!fwd "))
    Forward(getSpeed(cmd, 0));
  else if (cmd.startsWith("!bwd "))
    Backward(getSpeed(cmd, 0));
  else if (cmd.startsWith("!left "))
    TurnLeft(getSpeed(cmd, 0));
  else if (cmd.startsWith("!right "))
    TurnRight(getSpeed(cmd, 0));
  else if (cmd.startsWith("!fwdleft"))
    ForwardAndLeft(getSpeed(cmd, 0), getSpeed(cmd, 1));
  else if (cmd.startsWith("!fwdright"))
    ForwardAndRight(getSpeed(cmd, 0), getSpeed(cmd, 1));
  else if (cmd.startsWith("!bwdleft"))
    BackwardAndTurnLeft(getSpeed(cmd, 0), getSpeed(cmd, 1));
  else if (cmd.startsWith("!bwdright"))
    BackwardAndTurnRight(getSpeed(cmd, 0), getSpeed(cmd, 1));
  else
    Stop();
}

int getSpeed(String cmd, int idx)
{
  int mag = getCmdInt(cmd, idx);
  return g_speedFactor * mag + g_minSpeed;
}

int getCmdInt(String cmd, int idx)
{
  int spaceIdx = cmd.indexOf(' ');
  for (int i=0; i < idx; i++)
    spaceIdx = cmd.indexOf(' ', spaceIdx+1);

  return cmd.substring(spaceIdx+1).toInt();
}

bool cmdLineReady()
{
  if (g_bufReadIdx != 0)
    return false; // still building line

  if (g_lineBuf[0] != '!')
    return false; // not a command line

  return true;
}

void readIntoLineBuf(char c)
{
  if (c == '\n' || c == '\r' || c == '\0')
  {
    // finish line for processing, set ready for next line
    g_bufReadIdx = 0;
    return;
  }

  if (g_bufReadIdx >= MAXBUFSZ - 1)
  {
    // drop overflow bytes
    return;
  }

  g_lineBuf[g_bufReadIdx] = c;
  g_lineBuf[g_bufReadIdx+1] = '\0';
  g_bufReadIdx++;
}

void doUltrasonicCar()
{
  int distance = ultrasonic.distanceCm();
  
  boolean isBlocked = (distance > 0 && distance < 60);
  if (isBlocked)
    g_timeLastBlocked = millis();
  else if (millis() - g_timeLastBlocked < 100)
    isBlocked = true;

  if (isBlocked)
    digitalWrite(LED_PIN, HIGH);
  else
    digitalWrite(LED_PIN, LOW);

  boolean isStuck 
    = (g_numTurnSteps > g_maxTurnSteps
      || g_numStepsForward > g_maxStepsForward);

  if (g_ebrake == EBRAKE_STOP && !isBlocked)
  {
    // /unstuck, ready for go signal
    g_ebrake = EBRAKE_WAIT; 
    return;
  }
  else if (g_ebrake == EBRAKE_WAIT && isBlocked)
  {
    // "Go!"
    ResetUltrasonic(EBRAKE_GO);
    return;
  }
  else if (g_ebrake == EBRAKE_GO && isStuck)
  {
    // stuck -- yank ebrake
    g_ebrake = EBRAKE_STOP;
    Stop();
    return;
  }

  if (g_ebrake != EBRAKE_GO)
  {
    // halt until !blocked and receive "Go" signal
    return;
  }
  
  if (isBlocked)
  {
    // evasive maneuvers!
    randomSeed(analogRead(A4));
    int randnum = random(300);

    if (distance > 30)
    {
      if (randnum > 150 && !g_rightFlag)
        TurnLeft(g_moveSpeed);   
      else
        TurnRight(g_moveSpeed);  
    }
    else
    {
      if (randnum > 150) 
        BackwardAndTurnLeft(g_moveSpeed, g_moveSpeed);
      else 
        BackwardAndTurnRight(g_moveSpeed, g_moveSpeed);
    }
  }
  else
  {
    // ONWARD!
    Forward(g_moveSpeed);
  }
}

void ResetUltrasonic(int brakePos)
{
  g_rightFlag = false;
  g_numStepsForward = 0;
  g_numTurnSteps = 0;
  g_ebrake = brakePos;
  
  if (g_debug && g_mode == MODE_ULTRASONIC)
    Serial.println("Reset");
}

void Stop()
{
  MotorL.stop();
  MotorR.stop();
}

void Forward(int speed)
{
  g_numStepsForward++;
  g_numTurnSteps = 0;
  g_rightFlag = false;

  MotorL.run(speed * g_leftTrim);
  MotorR.run(speed * g_rightTrim);
  
  if (g_debug)
    Serial.println("Forward");
}

void ForwardAndLeft(int fspeed, int lspeed)
{
  g_numStepsForward++;
  g_numTurnSteps++;
  g_rightFlag = false;

  MotorL.run(fspeed - (lspeed / 2));
  MotorR.run(fspeed);
  
  if (g_debug)
    Serial.println("Forward+Left");
}

void ForwardAndRight(int fspeed, int rspeed)
{
  g_numStepsForward++;
  g_numTurnSteps++;
  g_rightFlag = true;

  MotorL.run(fspeed);
  MotorR.run(fspeed - (rspeed / 2));
  
  if (g_debug)
    Serial.println("Forward+Right");
}

void Backward(int speed)
{
  g_rightFlag = false;
  g_numTurnSteps = 0;
  MotorL.run(-speed);
  MotorR.run(-speed);

  if (g_debug)
    Serial.println("Backward");
}

void TurnLeft(int speed)
{
  g_rightFlag = false;
  g_numStepsForward = 0;
  g_numTurnSteps++;

  MotorL.run(-speed / 2);
  MotorR.run(speed / 2);
  
  if (g_debug)
    Serial.println("Left");
}

void TurnRight(int speed)
{
  g_rightFlag = true;
  g_numStepsForward = 0;
  g_numTurnSteps++;

  MotorL.run(speed / 2);
  MotorR.run(-speed / 2);
  
  if (g_debug)
    Serial.println("Right");
}

void BackwardAndTurnLeft(int bspeed, int lspeed)
{
  g_rightFlag = true;
  g_numStepsForward = 0;
  g_numTurnSteps++;

  MotorL.run(-bspeed + (lspeed / 2));
  MotorR.run(-bspeed);

  if (g_debug)
    Serial.println("Backward+Left");
}

void BackwardAndTurnRight(int bspeed, int rspeed)
{
  g_rightFlag = false;
  g_numStepsForward = 0;
  g_numTurnSteps++;

  MotorL.run(-bspeed);
  MotorR.run(-bspeed + (rspeed / 2));

  if (g_debug)
    Serial.println("Backward+Right");
}

void ChangeSpeed(int spd)
{
  g_moveSpeed = spd;
}
			
				
					
				
					1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
				
						#include <Makeblock.h>
#include <Arduino.h>
#include <String.h>
#include <SoftwareSerial.h>
#include <Wire.h>
 
MeDCMotor MotorL(M1);  
MeDCMotor MotorR(M2);
MeUltrasonicSensor ultrasonic(PORT_3);
 
MeBluetooth bluetooth(PORT_5);
 
const int MAXBUFSZ = 128;
char g_lineBuf[MAXBUFSZ]; 
int g_bufReadIdx = 0;
 
// movement settings
const boolean g_debug = true;
const int g_stepPeriod = 50;
const int g_maxStepsForward = 3000 / g_stepPeriod;
const int g_maxTurnSteps = 3000 / g_stepPeriod;
const int g_minSpeed = 45;
const int g_maxSpeed = 255;
const int g_speedFactor = 23;
const float g_leftTrim = .97;
const float g_rightTrim = 1.;
 
// enums
const int LED_PIN = 13;
const int EBRAKE_STOP = 2; // full stop
const int EBRAKE_WAIT = 1; // wait for "Go" signal
const int EBRAKE_GO = 0; // brakes off and GO!
const int MODE_ULTRASONIC = 0;
const int MODE_REMOTE = 1;
const int ULTRASONIC_INIT_SPEED = 200;
 
// robo state machine
int g_moveSpeed = ULTRASONIC_INIT_SPEED;
int g_numStepsForward = 0;
int g_numTurnSteps = 0;
boolean g_rightFlag;
int g_ebrake = EBRAKE_STOP;
long g_timeLastBlocked = 0;
uint8_t g_mode = MODE_REMOTE;
 
void setup()
{
  Serial.begin(9600);
  Serial.println("Software serial on");
 
  bluetooth.begin(9600);
  Serial.println("Bluetooth on");
 
  g_lineBuf[0] = '\0';
  ResetUltrasonic(EBRAKE_STOP);
  pinMode(LED_PIN, OUTPUT); // "obstructed" LED
 
  Serial.println("Bao ready");
}
 
void loop()
{
  char inDat;
  boolean hasBlue = false;
 
  hasBlue = bluetooth.available();
 
  if (hasBlue)
  {
    ResetUltrasonic(EBRAKE_GO);
    inDat = bluetooth.read();
    Serial.print(inDat);
 
    readIntoLineBuf(inDat);
    if (cmdLineReady())
    {
      String cmd(g_lineBuf);
      if (g_debug)
        Serial.println("COMMAND " + cmd);
      processCommand(cmd);
    }
  }
  else if (g_mode != MODE_REMOTE
    && millis() % g_stepPeriod == 0)
    doUltrasonicCar();
}
 
bool processCommand(String cmd)
{
  if (cmd == "!beep")
  {
    buzzerOn();
    delay(100);
    buzzerOff();
  }
  else if (cmd.startsWith("!fwd "))
    Forward(getSpeed(cmd, 0));
  else if (cmd.startsWith("!bwd "))
    Backward(getSpeed(cmd, 0));
  else if (cmd.startsWith("!left "))
    TurnLeft(getSpeed(cmd, 0));
  else if (cmd.startsWith("!right "))
    TurnRight(getSpeed(cmd, 0));
  else if (cmd.startsWith("!fwdleft"))
    ForwardAndLeft(getSpeed(cmd, 0), getSpeed(cmd, 1));
  else if (cmd.startsWith("!fwdright"))
    ForwardAndRight(getSpeed(cmd, 0), getSpeed(cmd, 1));
  else if (cmd.startsWith("!bwdleft"))
    BackwardAndTurnLeft(getSpeed(cmd, 0), getSpeed(cmd, 1));
  else if (cmd.startsWith("!bwdright"))
    BackwardAndTurnRight(getSpeed(cmd, 0), getSpeed(cmd, 1));
  else
    Stop();
}
 
int getSpeed(String cmd, int idx)
{
  int mag = getCmdInt(cmd, idx);
  return g_speedFactor * mag + g_minSpeed;
}
 
int getCmdInt(String cmd, int idx)
{
  int spaceIdx = cmd.indexOf(' ');
  for (int i=0; i < idx; i++)
    spaceIdx = cmd.indexOf(' ', spaceIdx+1);
 
  return cmd.substring(spaceIdx+1).toInt();
}
 
bool cmdLineReady()
{
  if (g_bufReadIdx != 0)
    return false; // still building line
 
  if (g_lineBuf[0] != '!')
    return false; // not a command line
 
  return true;
}
 
void readIntoLineBuf(char c)
{
  if (c == '\n' || c == '\r' || c == '\0')
  {
    // finish line for processing, set ready for next line
    g_bufReadIdx = 0;
    return;
  }
 
  if (g_bufReadIdx >= MAXBUFSZ - 1)
  {
    // drop overflow bytes
    return;
  }
 
  g_lineBuf[g_bufReadIdx] = c;
  g_lineBuf[g_bufReadIdx+1] = '\0';
  g_bufReadIdx++;
}
 
void doUltrasonicCar()
{
  int distance = ultrasonic.distanceCm();
  
  boolean isBlocked = (distance > 0 && distance < 60);
  if (isBlocked)
    g_timeLastBlocked = millis();
  else if (millis() - g_timeLastBlocked < 100)
    isBlocked = true;
 
  if (isBlocked)
    digitalWrite(LED_PIN, HIGH);
  else
    digitalWrite(LED_PIN, LOW);
 
  boolean isStuck 
    = (g_numTurnSteps > g_maxTurnSteps
      || g_numStepsForward > g_maxStepsForward);
 
  if (g_ebrake == EBRAKE_STOP && !isBlocked)
  {
    // /unstuck, ready for go signal
    g_ebrake = EBRAKE_WAIT; 
    return;
  }
  else if (g_ebrake == EBRAKE_WAIT && isBlocked)
  {
    // "Go!"
    ResetUltrasonic(EBRAKE_GO);
    return;
  }
  else if (g_ebrake == EBRAKE_GO && isStuck)
  {
    // stuck -- yank ebrake
    g_ebrake = EBRAKE_STOP;
    Stop();
    return;
  }
 
  if (g_ebrake != EBRAKE_GO)
  {
    // halt until !blocked and receive "Go" signal
    return;
  }
  
  if (isBlocked)
  {
    // evasive maneuvers!
    randomSeed(analogRead(A4));
    int randnum = random(300);
 
    if (distance > 30)
    {
      if (randnum > 150 && !g_rightFlag)
        TurnLeft(g_moveSpeed);   
      else
        TurnRight(g_moveSpeed);  
    }
    else
    {
      if (randnum > 150) 
        BackwardAndTurnLeft(g_moveSpeed, g_moveSpeed);
      else 
        BackwardAndTurnRight(g_moveSpeed, g_moveSpeed);
    }
  }
  else
  {
    // ONWARD!
    Forward(g_moveSpeed);
  }
}
 
void ResetUltrasonic(int brakePos)
{
  g_rightFlag = false;
  g_numStepsForward = 0;
  g_numTurnSteps = 0;
  g_ebrake = brakePos;
  
  if (g_debug && g_mode == MODE_ULTRASONIC)
    Serial.println("Reset");
}
 
void Stop()
{
  MotorL.stop();
  MotorR.stop();
}
 
void Forward(int speed)
{
  g_numStepsForward++;
  g_numTurnSteps = 0;
  g_rightFlag = false;
 
  MotorL.run(speed * g_leftTrim);
  MotorR.run(speed * g_rightTrim);
  
  if (g_debug)
    Serial.println("Forward");
}
 
void ForwardAndLeft(int fspeed, int lspeed)
{
  g_numStepsForward++;
  g_numTurnSteps++;
  g_rightFlag = false;
 
  MotorL.run(fspeed - (lspeed / 2));
  MotorR.run(fspeed);
  
  if (g_debug)
    Serial.println("Forward+Left");
}
 
void ForwardAndRight(int fspeed, int rspeed)
{
  g_numStepsForward++;
  g_numTurnSteps++;
  g_rightFlag = true;
 
  MotorL.run(fspeed);
  MotorR.run(fspeed - (rspeed / 2));
  
  if (g_debug)
    Serial.println("Forward+Right");
}
 
void Backward(int speed)
{
  g_rightFlag = false;
  g_numTurnSteps = 0;
  MotorL.run(-speed);
  MotorR.run(-speed);
 
  if (g_debug)
    Serial.println("Backward");
}
 
void TurnLeft(int speed)
{
  g_rightFlag = false;
  g_numStepsForward = 0;
  g_numTurnSteps++;
 
  MotorL.run(-speed / 2);
  MotorR.run(speed / 2);
  
  if (g_debug)
    Serial.println("Left");
}
 
void TurnRight(int speed)
{
  g_rightFlag = true;
  g_numStepsForward = 0;
  g_numTurnSteps++;
 
  MotorL.run(speed / 2);
  MotorR.run(-speed / 2);
  
  if (g_debug)
    Serial.println("Right");
}
 
void BackwardAndTurnLeft(int bspeed, int lspeed)
{
  g_rightFlag = true;
  g_numStepsForward = 0;
  g_numTurnSteps++;
 
  MotorL.run(-bspeed + (lspeed / 2));
  MotorR.run(-bspeed);
 
  if (g_debug)
    Serial.println("Backward+Left");
}
 
void BackwardAndTurnRight(int bspeed, int rspeed)
{
  g_rightFlag = false;
  g_numStepsForward = 0;
  g_numTurnSteps++;
 
  MotorL.run(-bspeed);
  MotorR.run(-bspeed + (rspeed / 2));
 
  if (g_debug)
    Serial.println("Backward+Right");
}
 
void ChangeSpeed(int spd)
{
  g_moveSpeed = spd;
}

The main changes were to handle commands sent via Bluetooth. Bytes were read one at a time and accumulated into a buffer. The buffer was parsed for commands and the commands were executed. I had to tune the vectors a little to take advantage of the analog joystick input and to smooth out transitions to/from any direction.

I rigged the smartphone onto the robot with Legos. Then I downloaded and streamed the robot’s POV using the “IP Webcam” app because why not?

It’s ALIVE! Robot POV

Pilot POV

It worked better than I expected. The controller lag was not too bad, even with the inefficient command string processing scheme.

Tee hee

When I was done playing I restored it back to the old IR + Ultrasonic setup. I still haven’t figured out what’s the best next step from IR to Bluetooth for a kid.

LG Optimus Dynamic – perfect cellbot phone?

November 13, 2014 by Aaron

It is the cheapest, full-featured Android phone that I’ve found. Only $20 on Amazon NEW. It feels like finding hidden treasure because the only reason it’s so cheap is that the phone is targeted for the bottom market. And, normal people who buy the phone later add minutes to use it as a phone. But not us! If everyone did what we want to do with the phone, they’d lose money because there’s no way this package can be produced and sold for a profit at $20. So shhhh… let’s just keep this between you and me.

Here’s what you do after you get it:

(optional) Root it. That link takes you to a super simple one-click script and all you need is a PC and the USB cable that came with the phone. I don’t know why I rooted mine, but it’s Android and you can, so why not? You could install this custom ROM too if you want (I didn’t). And if you screw something up like I did, you always go back to “stock” with this.
Install SL4A — the Scripting Layer for Android. This lets you interface with Android and everything your phone’s hardware allows… from a Python script.
* Install the APK
* Menu > view > interpreters
* Menu > add > Python
* Install the Python APK > OPEN the Python app > Install Python (+ whatever modules you want, like pySerial + pyBluez)
In SL4A, go to Menu > view > interpreters > Menu > start server
(Note the IP address and port, e.g. 192.168.1.3:12345)
From your PC, write a simple Python script called hello.py to talk to the server on your phone:
hello.py

import android droid = android.Android((192.168.1.3, 12345)) droid.ttsSpeak("hello world")

1
2
3
4
5

import android
droid = android.Android((192.168.1.3, 12345))
droid.ttsSpeak("hello world")
Notice the “import android” above. That’s from here. Download android.py and put it next to hello.py above.
python hello.py
CHEER

The hardware isn’t great and it’s still on Gingerbread, but I’m pretty sure it’s still more powerful than NASA’s Lunar Lander. It’s just a steal at $20 for the nice package. Battery life is super and everything works fine. It even has an external micro SD slot! Oh one thing you might want to upgrade for our example above is the Text-to-Speech voice. Gingerbread is old so you don’t get Google’s latest TTS software. So I downloaded IVONA. It’s free, it works for Gingerbread and I like Amy’s UK English voice.

By the way, the SL4A website taught me that I’m not the first brilliant person to think of using smartphones for robots. And, the term for robots that use smartphones is Cellbots. But it still makes me wonder why people still want to mess with things like a $30 XBee WiFi module. Especially when phones like the Optimus Dynamic gives you WiFi (plus everything else) for $20. Who knows.

Happy Hacking!

Robot Kit – Day 7

November 11, 2014 by Aaron

Our new ultrasonic car + IR code seems to work well, even in the living room. You only wanted to switch to MAX SPEED to watch it go nuts:

We made an interesting discovery: mama’s fuzzy bunny slippers are invisible to the ultrasonic sensor. I tried to demonstrate why by bouncing some balls off different surfaces. But I’m not sure if you got it.

PC to android smartphone recipe

November 10, 2014 by Aaron

Bao has been taking a break. I think having a robot that moves on its own stresses you out. You’ve been running to flip the off switch after playing with Bao a little. Maybe it’s too much for a 3 year old to deal with. On the plus side, now you know the deal with batteries and recharging, and how Bao gets slow when he’s low on energy. The “energy” and “battery” concept is handy for explaining why you have to eat and sleep.

Anyway, while Bao has been resting, we’ve moved on to the next phase of Bao’s evolution. After some research, I’ve found the perfect recipe. But first, check out the ingredients to see if you can guess what we’re making:

1) LG Optimus Dynamic
This is a $20 prepaid smartphone. Prepaid meaning you can actually buy minutes to use it as a smartphone (e.g. if you need a spare phone for family visiting from out of town). And this one’s nice because it has “triple minutes” for life. So you can buy 200 minutes and the phone will give you 600 minutes magically. But what I’m really buying this for is the hardware and software. It comes packed with the usual sensors: GPS, accelerometer, compass, light sensor; and communication interfaces: Bluetooth, WiFi, PLUS a camera, video recorder, audio recorder, a 1GHz processor, some RAM and 4GB of storage space, AND whatever apps you want and the Android OS to tie it all together. All for $20! In the robot world, each of these devices would cost $20 or more by themselves. But thanks to the economics of smartphones, you can get them ALL for $20. What a wonderful time we live in. I really feel that smartphone tech will accelerate many more industries. (Example: Oculus Rift.) So instead of buying loosey goosey electronics for your robo, the only thing you need is Bluetooth… to pair with a smartphone.

2) Scripting Layer for Android (SL4A)
This is what you install on your Android phone. SL4A lets you tap into every function on your phone from your favorite scripting language (e.g., Python). SL4A can either run a script stored on the phone, or you can connect to SL4A to run a script from your PC. E.g., check this out:

From your PC:



		
		
			
			

			
			import android
droid = android.Android(('192.168.1.3', 53833))
recog = droid.recognizeSpeech().result
droid.ttsSpeak(recog)

			
				
					
				
					1
2
3
4
5
				
						import android
droid = android.Android(('192.168.1.3', 53833))
recog = droid.recognizeSpeech().result
droid.ttsSpeak(recog)
 
					
				
			
		


This tells your phone at 192.168.1.3 to, "Capture what I say, then say it back to me." How crazy is that? (Browse the API to see what else it can do.) You can tell it to take a picture, make a phone call, or even drive a robot... (hint hint)
3) web2py

This is what I used to make a toddler-proof web interface or "app" or GUI. The front-end (View) is HTML/Javascript, and the backend (Controller) is Python -- perfect for hooking up with SL4A. Right now, it doesn't do much. But it still has you rolling on the floor laughing, because I send the words you type to the phone and the phone says them (using SL4A). You've been having fun typing different words to see how the text-to-speech voice screws it up. Anyway, this bomb-proof custom toddler web UI works much better than VI, which we were using earlier...


Oh, and here's my latest IR + Ultrasonic car code. It's tuned for the 3-wheel car and my kitchen:


		
		
			
			

			
			
#include <Makeblock.h>
#include <SoftwareSerial.h>
#include <Wire.h>

MeDCMotor MotorL(M1);  
MeDCMotor MotorR(M2);
MeUltrasonicSensor UltrasonicSensor(PORT_3);
MeInfraredReceiver infraredReceiverDecode(PORT_6);

const boolean g_debug = true;
const int g_stepPeriod = 50;
const int g_maxStepsForward = 3000 / g_stepPeriod;
const int g_maxTurnSteps = 3000 / g_stepPeriod;
const int g_minSpeed = 45;
const float g_leftTrim = .97;
const float g_rightTrim = 1.;

const int LED_PIN = 13;
const int EBRAKE_STOP = 2; // full stop
const int EBRAKE_WAIT = 1; // wait for "Go" signal
const int EBRAKE_GO = 0; // brakes off and GO!
const int MODE_ULTRASONIC = 0;
const int ULTRASONIC_INIT_SPEED = 150;
const int MODE_IR = 1;

int g_speedFactor = 23;
int g_moveSpeed = ULTRASONIC_INIT_SPEED;
int g_numStepsForward = 0;
int g_numTurnSteps = 0;
boolean g_rightFlag;
int g_ebrake = EBRAKE_STOP;
long g_timeLastBlocked = 0;

uint8_t g_mode = MODE_IR;

void setup()
{
  if (g_debug)
    Serial.begin(9600);

  ResetUltrasonic(EBRAKE_STOP);

  infraredReceiverDecode.begin(); 
  pinMode(LED_PIN, OUTPUT); // "obstructed" LED
}

void loop()
{
  if (infraredReceiverDecode.available() || infraredReceiverDecode.buttonState())
  {
    ResetUltrasonic(EBRAKE_GO);
    switch (infraredReceiverDecode.read())
    {
      case IR_BUTTON_PLUS: 
        Forward();
        break;
      case IR_BUTTON_MINUS:
        Backward();
        break;
      case IR_BUTTON_NEXT:
        TurnRight();
        break;
      case IR_BUTTON_PREVIOUS:
        TurnLeft();
        break;
      case IR_BUTTON_TEST:
        ForwardAndLeft();
        break;
      case IR_BUTTON_RETURN:
        ForwardAndRight();
        break;
      case IR_BUTTON_9:
        ChangeSpeed(g_speedFactor*9+g_minSpeed);
        break;
      case IR_BUTTON_8:
        ChangeSpeed(g_speedFactor*8+g_minSpeed);
        break;
      case IR_BUTTON_7:
        ChangeSpeed(g_speedFactor*7+g_minSpeed);
        break;
      case IR_BUTTON_6:
        ChangeSpeed(g_speedFactor*6+g_minSpeed);
        break;
      case IR_BUTTON_5:
        ChangeSpeed(g_speedFactor*5+g_minSpeed);
        break;
      case IR_BUTTON_4:
        ChangeSpeed(g_speedFactor*4+g_minSpeed);
        break;
      case IR_BUTTON_3:
        ChangeSpeed(g_speedFactor*3+g_minSpeed);
        break;
      case IR_BUTTON_2:
        ChangeSpeed(g_speedFactor*2+g_minSpeed);
        break;
      case IR_BUTTON_1:
        ChangeSpeed(g_speedFactor*1+g_minSpeed);
        break;
      case IR_BUTTON_POWER:
        buzzerOn();
        delay(100);
        buzzerOff();
        break;
      case IR_BUTTON_MENU:
        if (g_mode == MODE_IR)
        {
          buzzerOn();
          delay(100);
          buzzerOff();
          delay(100);
          buzzerOn();
          delay(100);
          buzzerOff();
          delay(100);
          buzzerOn();
          delay(300);
          buzzerOff();

          g_mode = MODE_ULTRASONIC;
          g_moveSpeed = ULTRASONIC_INIT_SPEED;
        }
        else
        {
          buzzerOn();
          delay(100);
          buzzerOff();
          digitalWrite(LED_PIN, LOW);

          g_mode = MODE_IR;
        }

        break;
      default:
        break;
    }
  }
  else
  {
    if (g_mode == MODE_IR)
      Stop();
    else if (millis() % g_stepPeriod == 0)
      doUltrasonicCar();
  }
}

void doUltrasonicCar()
{
  int distance = UltrasonicSensor.distanceCm();
  
  boolean isBlocked = (distance > 0 && distance < 60);
  if (isBlocked)
    g_timeLastBlocked = millis();
  else if (millis() - g_timeLastBlocked < 100)
    isBlocked = true;

  if (isBlocked)
    digitalWrite(LED_PIN, HIGH);
  else
    digitalWrite(LED_PIN, LOW);

  boolean isStuck 
    = (g_numTurnSteps > g_maxTurnSteps
      || g_numStepsForward > g_maxStepsForward);

  if (g_ebrake == EBRAKE_STOP && !isBlocked)
  {
    // /unstuck, ready for go signal
    g_ebrake = EBRAKE_WAIT; 
    return;
  }
  else if (g_ebrake == EBRAKE_WAIT && isBlocked)
  {
    // "Go!"
    ResetUltrasonic(EBRAKE_GO);
    return;
  }
  else if (g_ebrake == EBRAKE_GO && isStuck)
  {
    // stuck -- yank ebrake
    g_ebrake = EBRAKE_STOP;
    Stop();
    return;
  }

  if (g_ebrake != EBRAKE_GO)
  {
    // halt until !blocked and receive "Go" signal
    return;
  }
  
  if (isBlocked)
  {
    // evasive maneuvers!
    randomSeed(analogRead(A4));
    int randnum = random(300);

    if (distance > 30)
    {
      if (randnum > 190 && !g_rightFlag)
        TurnLeft();   
      else
        TurnRight();  
    }
    else
    {
      if (randnum > 190) 
        BackwardAndTurnLeft();
      else 
        BackwardAndTurnRight();
    }
  }
  else
  {
    // ONWARD!
    Forward();
  }
}

void ResetUltrasonic(int brakePos)
{
  g_rightFlag = false;
  g_numStepsForward = 0;
  g_numTurnSteps = 0;
  g_ebrake = brakePos;
  
  if (g_debug && g_mode == 0)
    Serial.println("Reset");
}

void Stop()
{
  MotorL.stop();
  MotorR.stop();
  
  if (g_debug)
    Serial.println("Stop");
}

void Forward()
{
  g_numStepsForward++;
  g_numTurnSteps = 0;
  g_rightFlag = false;

  MotorL.run(g_moveSpeed * g_leftTrim);
  MotorR.run(g_moveSpeed * g_rightTrim);
  
  if (g_debug)
    Serial.println("Forward");
}

void ForwardAndLeft()
{
  g_numStepsForward++;
  g_numTurnSteps++;
  g_rightFlag = false;

  MotorL.run(g_moveSpeed / 2);
  MotorR.run(g_moveSpeed);
  
  if (g_debug)
    Serial.println("Forward+Left");
}

void ForwardAndRight()
{
  g_numStepsForward++;
  g_numTurnSteps++;
  g_rightFlag = true;

  MotorL.run(g_moveSpeed);
  MotorR.run(g_moveSpeed / 2);
  
  if (g_debug)
    Serial.println("Forward+Right");
}

void Backward()
{
  g_rightFlag = false;
  g_numTurnSteps = 0;
  MotorL.run(-g_moveSpeed);
  MotorR.run(-g_moveSpeed);
}
void TurnLeft()
{
  g_rightFlag = false;
  g_numStepsForward = 0;
  g_numTurnSteps++;

  MotorL.run(-g_moveSpeed);
  MotorR.run(g_moveSpeed);
  
  if (g_debug)
    Serial.println("Left");
}
void TurnRight()
{
  g_rightFlag = true;
  g_numStepsForward = 0;
  g_numTurnSteps++;

  MotorL.run(g_moveSpeed);
  MotorR.run(-g_moveSpeed);
  
  if (g_debug)
    Serial.println("Right");
}

void BackwardAndTurnLeft()
{
  g_rightFlag = true;
  g_numStepsForward = 0;
  g_numTurnSteps++;

  MotorL.run(-g_moveSpeed/2);
  MotorR.run(-g_moveSpeed);

  if (g_debug)
    Serial.println("Backward+Left");
}
void BackwardAndTurnRight()
{
  g_rightFlag = false;
  g_numStepsForward = 0;
  g_numTurnSteps++;

  MotorL.run(-g_moveSpeed);
  MotorR.run(-g_moveSpeed/2);

  if (g_debug)
    Serial.println("Backward+Right");
}

void ChangeSpeed(int spd)
{
  g_moveSpeed = spd;
}
			
				
					
				
					1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
				
						 
#include <Makeblock.h>
#include <SoftwareSerial.h>
#include <Wire.h>
 
MeDCMotor MotorL(M1);  
MeDCMotor MotorR(M2);
MeUltrasonicSensor UltrasonicSensor(PORT_3);
MeInfraredReceiver infraredReceiverDecode(PORT_6);
 
const boolean g_debug = true;
const int g_stepPeriod = 50;
const int g_maxStepsForward = 3000 / g_stepPeriod;
const int g_maxTurnSteps = 3000 / g_stepPeriod;
const int g_minSpeed = 45;
const float g_leftTrim = .97;
const float g_rightTrim = 1.;
 
const int LED_PIN = 13;
const int EBRAKE_STOP = 2; // full stop
const int EBRAKE_WAIT = 1; // wait for "Go" signal
const int EBRAKE_GO = 0; // brakes off and GO!
const int MODE_ULTRASONIC = 0;
const int ULTRASONIC_INIT_SPEED = 150;
const int MODE_IR = 1;
 
int g_speedFactor = 23;
int g_moveSpeed = ULTRASONIC_INIT_SPEED;
int g_numStepsForward = 0;
int g_numTurnSteps = 0;
boolean g_rightFlag;
int g_ebrake = EBRAKE_STOP;
long g_timeLastBlocked = 0;
 
uint8_t g_mode = MODE_IR;
 
void setup()
{
  if (g_debug)
    Serial.begin(9600);
 
  ResetUltrasonic(EBRAKE_STOP);
 
  infraredReceiverDecode.begin(); 
  pinMode(LED_PIN, OUTPUT); // "obstructed" LED
}
 
void loop()
{
  if (infraredReceiverDecode.available() || infraredReceiverDecode.buttonState())
  {
    ResetUltrasonic(EBRAKE_GO);
    switch (infraredReceiverDecode.read())
    {
      case IR_BUTTON_PLUS: 
        Forward();
        break;
      case IR_BUTTON_MINUS:
        Backward();
        break;
      case IR_BUTTON_NEXT:
        TurnRight();
        break;
      case IR_BUTTON_PREVIOUS:
        TurnLeft();
        break;
      case IR_BUTTON_TEST:
        ForwardAndLeft();
        break;
      case IR_BUTTON_RETURN:
        ForwardAndRight();
        break;
      case IR_BUTTON_9:
        ChangeSpeed(g_speedFactor*9+g_minSpeed);
        break;
      case IR_BUTTON_8:
        ChangeSpeed(g_speedFactor*8+g_minSpeed);
        break;
      case IR_BUTTON_7:
        ChangeSpeed(g_speedFactor*7+g_minSpeed);
        break;
      case IR_BUTTON_6:
        ChangeSpeed(g_speedFactor*6+g_minSpeed);
        break;
      case IR_BUTTON_5:
        ChangeSpeed(g_speedFactor*5+g_minSpeed);
        break;
      case IR_BUTTON_4:
        ChangeSpeed(g_speedFactor*4+g_minSpeed);
        break;
      case IR_BUTTON_3:
        ChangeSpeed(g_speedFactor*3+g_minSpeed);
        break;
      case IR_BUTTON_2:
        ChangeSpeed(g_speedFactor*2+g_minSpeed);
        break;
      case IR_BUTTON_1:
        ChangeSpeed(g_speedFactor*1+g_minSpeed);
        break;
      case IR_BUTTON_POWER:
        buzzerOn();
        delay(100);
        buzzerOff();
        break;
      case IR_BUTTON_MENU:
        if (g_mode == MODE_IR)
        {
          buzzerOn();
          delay(100);
          buzzerOff();
          delay(100);
          buzzerOn();
          delay(100);
          buzzerOff();
          delay(100);
          buzzerOn();
          delay(300);
          buzzerOff();
 
          g_mode = MODE_ULTRASONIC;
          g_moveSpeed = ULTRASONIC_INIT_SPEED;
        }
        else
        {
          buzzerOn();
          delay(100);
          buzzerOff();
          digitalWrite(LED_PIN, LOW);
 
          g_mode = MODE_IR;
        }
 
        break;
      default:
        break;
    }
  }
  else
  {
    if (g_mode == MODE_IR)
      Stop();
    else if (millis() % g_stepPeriod == 0)
      doUltrasonicCar();
  }
}
 
void doUltrasonicCar()
{
  int distance = UltrasonicSensor.distanceCm();
  
  boolean isBlocked = (distance > 0 && distance < 60);
  if (isBlocked)
    g_timeLastBlocked = millis();
  else if (millis() - g_timeLastBlocked < 100)
    isBlocked = true;
 
  if (isBlocked)
    digitalWrite(LED_PIN, HIGH);
  else
    digitalWrite(LED_PIN, LOW);
 
  boolean isStuck 
    = (g_numTurnSteps > g_maxTurnSteps
      || g_numStepsForward > g_maxStepsForward);
 
  if (g_ebrake == EBRAKE_STOP && !isBlocked)
  {
    // /unstuck, ready for go signal
    g_ebrake = EBRAKE_WAIT; 
    return;
  }
  else if (g_ebrake == EBRAKE_WAIT && isBlocked)
  {
    // "Go!"
    ResetUltrasonic(EBRAKE_GO);
    return;
  }
  else if (g_ebrake == EBRAKE_GO && isStuck)
  {
    // stuck -- yank ebrake
    g_ebrake = EBRAKE_STOP;
    Stop();
    return;
  }
 
  if (g_ebrake != EBRAKE_GO)
  {
    // halt until !blocked and receive "Go" signal
    return;
  }
  
  if (isBlocked)
  {
    // evasive maneuvers!
    randomSeed(analogRead(A4));
    int randnum = random(300);
 
    if (distance > 30)
    {
      if (randnum > 190 && !g_rightFlag)
        TurnLeft();   
      else
        TurnRight();  
    }
    else
    {
      if (randnum > 190) 
        BackwardAndTurnLeft();
      else 
        BackwardAndTurnRight();
    }
  }
  else
  {
    // ONWARD!
    Forward();
  }
}
 
void ResetUltrasonic(int brakePos)
{
  g_rightFlag = false;
  g_numStepsForward = 0;
  g_numTurnSteps = 0;
  g_ebrake = brakePos;
  
  if (g_debug && g_mode == 0)
    Serial.println("Reset");
}
 
void Stop()
{
  MotorL.stop();
  MotorR.stop();
  
  if (g_debug)
    Serial.println("Stop");
}
 
void Forward()
{
  g_numStepsForward++;
  g_numTurnSteps = 0;
  g_rightFlag = false;
 
  MotorL.run(g_moveSpeed * g_leftTrim);
  MotorR.run(g_moveSpeed * g_rightTrim);
  
  if (g_debug)
    Serial.println("Forward");
}
 
void ForwardAndLeft()
{
  g_numStepsForward++;
  g_numTurnSteps++;
  g_rightFlag = false;
 
  MotorL.run(g_moveSpeed / 2);
  MotorR.run(g_moveSpeed);
  
  if (g_debug)
    Serial.println("Forward+Left");
}
 
void ForwardAndRight()
{
  g_numStepsForward++;
  g_numTurnSteps++;
  g_rightFlag = true;
 
  MotorL.run(g_moveSpeed);
  MotorR.run(g_moveSpeed / 2);
  
  if (g_debug)
    Serial.println("Forward+Right");
}
 
void Backward()
{
  g_rightFlag = false;
  g_numTurnSteps = 0;
  MotorL.run(-g_moveSpeed);
  MotorR.run(-g_moveSpeed);
}
void TurnLeft()
{
  g_rightFlag = false;
  g_numStepsForward = 0;
  g_numTurnSteps++;
 
  MotorL.run(-g_moveSpeed);
  MotorR.run(g_moveSpeed);
  
  if (g_debug)
    Serial.println("Left");
}
void TurnRight()
{
  g_rightFlag = true;
  g_numStepsForward = 0;
  g_numTurnSteps++;
 
  MotorL.run(g_moveSpeed);
  MotorR.run(-g_moveSpeed);
  
  if (g_debug)
    Serial.println("Right");
}
 
void BackwardAndTurnLeft()
{
  g_rightFlag = true;
  g_numStepsForward = 0;
  g_numTurnSteps++;
 
  MotorL.run(-g_moveSpeed/2);
  MotorR.run(-g_moveSpeed);
 
  if (g_debug)
    Serial.println("Backward+Left");
}
void BackwardAndTurnRight()
{
  g_rightFlag = false;
  g_numStepsForward = 0;
  g_numTurnSteps++;
 
  MotorL.run(-g_moveSpeed);
  MotorR.run(-g_moveSpeed/2);
 
  if (g_debug)
    Serial.println("Backward+Right");
}
 
void ChangeSpeed(int spd)
{
  g_moveSpeed = spd;
}

Robot Kit – Day Six

November 6, 2014 by Aaron

Oh the wonderful things a robot can do

It can go CHOO CHOO

It can go BEEP BEEP

I’m going to resist making any more mods until you suggest them. I also want to give mama a chance to tinker. Because she said, “But I wanted to do the BEEP!”

One thing we might have to do soon is give Bao a bumper. The ultrasonic sensor takes a beating from all the crashes. Anyway, with the new 3-wheel configuration, we haven’t had to recharge it for 3 days. The 4-wheel tank ran empty in just one day. But then again, we’ve only been in IR mode. Maybe the Ultrasonic processing is a power hog too.

And finally, it was time for new batteries. I strapped some Legos to the frame. You put "one door for going in, and one door for going out."

And finally, it was time for new batteries. Just for fun, I strapped some Legos to the frame. You made “one door for going in, and one door for going out.”

Robot Kit – Day Six (“What’s that sound downstairs?”)

November 3, 2014 by Aaron

What’s that sound coming from downstairs?

“I want his name to still be Bawa.”

Bonus: The mailman brought gifts! An 8-pack of Eneloops and one fast charger. I got that charger because it has a DC 12V input, which can be driven by cigarette lighter sockets and a foldable solar panel I’ve been thinking of getting.

Choo choo!

Mama gets a turn too, somewhat. And we learn to share

Secret Science Lab

Surprising adventures with science and DIY tech!

Author Archives: Aaron

Secret Spyno: How to add a new agent

Arduino Vader “Force Grip” remote car (a.k.a. smartphone virtual wheel)

Teaser: Makeblock Robot Starter Kit Cellbot + Sensors + shxtty Path Tracking

How to fix SL4A webCamFacade exception

How to build a Makeblock Bluetooth/WiFi Cellbot car

LG Optimus Dynamic – perfect cellbot phone?

Robot Kit – Day 7

PC to android smartphone recipe

Robot Kit – Day Six

Robot Kit – Day Six (“What’s that sound downstairs?”)