By lyweilian
#1488
I thought I would share my robot project with you guys and maybe get some feedback on possible improvements.

This robot uses the 4WD DFRobot Platform with a Romeo brain, an ethernet shield, 3 IR switch sensors and a router strapped to the top; and communicates via OSC to an IPhone.

The application I used on the IPhone is TouchOSC and I purchased it from the Apple App Store for $4.99. I chose this application because it has a nifty little editor that allows me to drop controls and configure with much ease.

To control the robot I dropped two Fader controls on to my TouchOSC template which allows me to slide the fader up and down to generate the PWM values that I need to drive the motors.
ie: -255 for full reverse to 255 for full forward.

The TouchOSC application not only has the ability to send OSC signals, but also recieve them so I can get feedback from the robot such confirmed PWM values, "demo mode", "Object Avoidance Mode" and sensor triggers.

The Arduino code comes from various examples from the DFRobot wiki and simple "GOOGLING" the Internet.
[img]http://farm8.staticflickr.com/7206/7031 ... 5e65_c.jpg[/img]
[img]http://farm8.staticflickr.com/7120/7031 ... ae33_c.jpg[/img]
[img]http://farm8.staticflickr.com/7217/7031 ... 8be9_c.jpg[/img]
Here is a link to a video http://www.youtube.com/embed/rcGGqQ3HylQ

Here is a link to a photo stream http://www.flickr.com/photos/[email protected]/

I'm also sharing my code in case it helps anyone out or if you have any comments for improvement. Any would be appreciated.
Code: Select all
#include <SPI.h>
#include <Ethernet.h>
#include <ArdOSC.h>

const int E1 = 5;     //M1 Speed Control
const int E2 = 6;     //M2 Speed Control
const int M1 = 4;    //M1 Direction Control
const int M2 = 7;    //M1 Direction Control
const int FrontInfraredSensorPin = 9;
const int LeftInfraredSensorPin = 8;
const int RightInfraredSensorPin = 2;

int serverPort = 9000;
int destPort = 8000;
byte destIp[]  = { 
  10, 0, 0, 2 };
byte myMac[] = { 
  0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED };
byte myIp[]  = { 
  10, 0, 0, 250 };
boolean lastState = false;           //Keeps track of when we go between enabled/disabled or vice versa   
unsigned long lastUpdate = 0;        //Keeps track of the last time (ms) we received data
boolean autoMode = false;
boolean debugMode = false;
float lastLeftPWM = 0;
float lastRightPWM = 0;

OSCServer server;
OSCClient client;

void setup(void) 
{ 
  int i;
  for(i=4;i<=7;i++)
    pinMode(i, OUTPUT); 

  pinMode(FrontInfraredSensorPin,INPUT);
  pinMode(LeftInfraredSensorPin,INPUT);
  pinMode(RightInfraredSensorPin,INPUT);
  Ethernet.begin(myMac ,myIp); 
  server.begin(serverPort);
  server.addCallback("/ard/Left",&func1);
  server.addCallback("/ard/Right",&func2);
  server.addCallback("/ard/autoMode",&func3);
  server.addCallback("/ard/debugMode",&func4);
  SendMessage("System Ready!");
} 
void loop(void) 
{
  if(server.aviableCheck()>0){
    lastUpdate = millis();
  }
  //  if (((millis() - lastUpdate) <= 100) && (millis() > 500))  //Robot is disabled for first 500ms of runtime
  //  {
  //    if (lastState == false) {
  //      Drive();
  //    }
  //  }
  //  else
  //  {
  //    disabled();  
  //  }
  if(autoMode)
  {
    SendMessage("Auto Mode!");
    AutoMode();
  }
  else
  {
    Drive();
  }
  ReportSensors();
}
void disabled()
{
  stop();
  lastState = false;
}
void func1(OSCMessage *_mes){
  lastLeftPWM =_mes->getArgFloat(0);
}
void func2(OSCMessage *_mes){
  lastRightPWM =_mes->getArgFloat(0);
}
//Sets object avoidance mode
void func3(OSCMessage *_mes){
  int statusId =(int)_mes->getArgFloat(0);
  SendMessage("AUTOMODE: " + statusId);
  autoMode = statusId == 1;
}
//Sets debug mode
void func4(OSCMessage *_mes){
  int statusId = (int)_mes->getArgFloat(0);
  SendMessage("DEBUGMODE: " + String(statusId));
  debugMode = statusId == 1;
}

void SendMessage(String message){
  message + "\n";
  char statusMessage[message.length()];
  for (int a=0;a<=message.length();a++)
  {
    statusMessage[a]=message[a];
  }
  OSCMessage loacal_mes;
  loacal_mes.setAddress(destIp,destPort);
  loacal_mes.beginMessage("/ard/message");  
  loacal_mes.addArgString(statusMessage);
  client.send(&loacal_mes);
}
void SendMessage(String message, int oscControl){
  message + "\n";
  char statusMessage[message.length()];
  for (int a=0;a<=message.length();a++)
  {
    statusMessage[a]=message[a];
  }
      
  OSCMessage loacal_mes;
  loacal_mes.setAddress(destIp,destPort);
  
  switch(oscControl)
  {
    case 1:
    loacal_mes.beginMessage("/ard/frontLed");
    break;
    case 2:
    loacal_mes.beginMessage("/ard/leftLed");
    break;
    case 3:
    loacal_mes.beginMessage("/ard/rightLed");
    break;
  }  
  
  loacal_mes.addArgString(statusMessage);
  client.send(&loacal_mes);
}
void AutoMode()
{
  if(!IsSensorClear(FrontInfraredSensorPin))
  {
    if(!debugMode)
    {
      Reverse(255,255);
    }
    LeftRightNegotiate();
  }
  else
  {
    if(!debugMode)
    {
      Forward(255,255);
    }
    LeftRightNegotiate();
  }
}
void ReportSensors()
{
  SendMessage(IsSensorClear(FrontInfraredSensorPin) ? "0" : "1",1); 
  SendMessage(IsSensorClear(LeftInfraredSensorPin) ? "0" : "1",2); 
  SendMessage(IsSensorClear(RightInfraredSensorPin) ? "0" : "1",3); 
}
void LeftRightNegotiate()
{
  if(!IsSensorClear(LeftInfraredSensorPin))
  {
    if(!debugMode)
    {
      Right(255,255);
      delay(100);
    }
  }
  if(!IsSensorClear(RightInfraredSensorPin))
  {
    if(!debugMode)
    {
      Left(255,255);
      delay(100);
    }
  }
}

void Drive ()
{
  String pwmStatus = "PWM: " + String((int)lastLeftPWM) + "," + String((int)lastRightPWM);
  if(debugMode)
  {
    SendMessage(pwmStatus);
  }
  else
  {
    if((lastLeftPWM <= 50 &&  lastLeftPWM >= -50) && (lastRightPWM <= 50 &&  lastRightPWM >= -50))
    {
      stop();
      SendMessage("Status: Stopped");
    }
    else
    {
      analogWrite (E1,abs(lastLeftPWM));
      analogWrite (E2,abs(lastRightPWM)); 
      SendMessage(pwmStatus);
      if(lastLeftPWM > 0)
      {
        digitalWrite(M1,LOW);    
      }
      else
      {
        digitalWrite(M1,HIGH);  
      }

      if(lastRightPWM > 0)
      {
        digitalWrite(M2,HIGH);
      }
      else
      {
        digitalWrite(M2,LOW);
      }
    }
    lastState = true;
  }
}

void stop(void) 
{
  digitalWrite(E1,LOW);   
  digitalWrite(E2,LOW);      
} 

void Left(char a,char b)
{
  analogWrite (E1,a); 
  digitalWrite(M1,HIGH);    
  analogWrite (E2,b);    
  digitalWrite(M2,HIGH);
}  
void Right (char a,char b) 
{
  analogWrite (E1,a);
  digitalWrite(M1,LOW);   
  analogWrite (E2,b);    
  digitalWrite(M2,LOW);
}
void Forward (char a,char b)
{
  analogWrite (E1,a);
  digitalWrite(M1,LOW);    
  analogWrite (E2,b);    
  digitalWrite(M2,HIGH);
}
void Reverse (char a,char b)
{
  analogWrite (E1,a);
  digitalWrite(M1,HIGH);    
  analogWrite (E2,b);    
  digitalWrite(M2,LOW);
}
boolean IsSensorClear(int pin)
{
  return digitalRead(pin) == HIGH;
}