The Arduino code basically has 2 purposes. First and foremost is to communicate with the PC, allowing me to write a complex brain using a high powered computer that controls the robot remotely. Second to that is to act as an auto pilot for when the pc connection gets lost or (more likely) some bug in my pc code causes it to stop responding.
I've divided the code up into 2 'modes'. One is a simple text based mode that allows me to take control over the robot via hyper terminal. The second is a more advanced mode that has more commands and uses binary data to communicate:
///////////////////////////////////////////////////////////////////
//Main loop in advanced mode
///////////////////////////////////////////////////////////////////
void LoopAdvancedMode()
{
//read incoming binary commands
ProcessCommands();
//do advanced logic and autopilot
}
///////////////////////////////////////////////////////////////////
//Main loop in simple (text) mode
///////////////////////////////////////////////////////////////////
void LoopSimpleMode()
{
//simple text based commands + logic here
while(Serial1.available())
{
int val = Serial1.read();
if(val == '8')
{
//blabla
}
//lots more if statements
//special case for switching to advanced mode
else if(val == 'T')
{
GMode = 1;
}
}
///////////////////////////////////////////////////////////////////
//Main loop - just calls simple or advanced version
///////////////////////////////////////////////////////////////////
void loop()
{
if(GMode == 0)
{
LoopSimpleMode();
}
else
{
LoopAdvancedMode();
}
}
Basically, the main loop either calls the simple loop function, or the advanced loop function. It defaults to simple mode, however you send it 't' to switch to advanced mode. Once in advanced mode you can send it a command to switch back into simple mode.
Interestingly, the code has a lot in common with standard game code. For example, it can not stall at any point. In a game this is to avoid ugly frame rate issues. However the MmBot could cause herself (or others) damage if left in a blocking state, as the motors may be running at the time and she could drive straight into a wall. Just like in a game the code needs to be able to receive commands over a network (aka the blu tooth) and respond to them in an entirely none blocking way. It needs to respond to keep alive requests and handle scenarios where a connection is lost. In a game (especially one like LBP) you can never predict all the possible problems the user will create, so you have to design for things you haven't thought of! In just the same way, the MmBot will be driving around a constantly changing and unpredictable world so needs to respond quickly - or totally fail to respond correctly but do it in a cute way!
This code shows the command processing:
enum ECommands
{
COMMAND_KEEPALIVE,
COMMAND_PING,
COMMAND_MOTION,
COMMAND_COMPASS,
COMMAND_SAY,
COMMAND_SET_SENSOR_POSITION,
COMMAND_FORWARDS,
COMMAND_LEFT,
COMMAND_RIGHT,
COMMAND_HORIZONTAL_SENSOR_SWEEP,
COMMAND_SIMPLE_MODE,
TOTAL_COMMANDS
};
ECommands GCurrentCommand = TOTAL_COMMANDS; //command currently being read
///////////////////////////////////////////////////////////////////
//reads commands from pc
///////////////////////////////////////////////////////////////////
void ProcessCommands()
{
//if not currently reading a command, check if there is one available to be read
if(GCurrentCommand == TOTAL_COMMANDS)
{
if(Serial1.available() >= 2)
{
//read the new command
GCurrentCommand = (ECommands)ReadWord();
}
}
//will now execute the command, assuming there's enough data available
switch(GCurrentCommand)
{
case COMMAND_SET_SENSOR_POSITION:
{
//sets sensor positions - requires 2 integers so doesn't execute until 4 bytes are available
if(Serial1.available() >= 4)
{
ServoTargetX = ReadWord();
ServoTargetY = ReadWord();
GCurrentCommand = TOTAL_COMMANDS;
}
break;
}
case COMMAND_SIMPLE_MODE:
{
//switches back to simple mode
GMode = 0;
GCurrentCommand = TOTAL_COMMANDS;
break;
}
case COMMAND_FORWARDS:
{
//move forwards and return 1
LeftMotorActiveTimer = 10;
RightMotorActiveTimer = 10;
WriteWord(1);
GCurrentCommand = TOTAL_COMMANDS;
}
break;
///////////////////////////////////////////////////////////
/// SIMILAR STUFF FOR ALL THE OTHER COMMAND TYPES HERE
default:
GCurrentCommand = TOTAL_COMMANDS;
break;
}
}
The key here is that it never blocks. Each frame, if no commands are being processed, it checks if 2 bytes are available (the size of a command code). Once a command code is received it'll check each frame to see if all the data required to execute the command is available. Many commands need no extra data, however with something like 'set sensor position' you needs 4 bytes to know where to move to. Using this none blocking approach the robot can maintain constant control over the motors.
Speaking of motors, they too have a safety mechanism built in. Rather than directly turning motors on/off, the commands simply request to keep the motors on for another few frames. You can see how it works from this function that updates the motors / servos:
///////////////////////////////////////////////////////////////////
//updates the servos and motors
///////////////////////////////////////////////////////////////////
void UpdateMotorTargets()
{
SensorServoX.write(ServoTargetX);
SensorServoY.write(ServoTargetY);
if(LeftMotorActiveTimer > 0)
{
digitalWrite(PIN_MOTOR_LEFT,HIGH);
LeftMotorActiveTimer--;
}
else
{
digitalWrite(PIN_MOTOR_LEFT,LOW);
}
if(RightMotorActiveTimer > 0)
{
digitalWrite(PIN_MOTOR_RIGHT,HIGH);
RightMotorActiveTimer--;
}
else
{
digitalWrite(PIN_MOTOR_RIGHT,LOW);
}
}
This 'UpdateMotorTargets' function is called once per frame in either mode. Providing nothing actually blocks, this means that the motors will be turned off by default unless they are repeatedly told to stay on. All with the intent of ensuring that in the event of epic failure, the first thing MmBot does is stop moving!
Anyhoo, that's the lot for today. Tomorrow I'll either get a bit more coding done, or if wires arrive, get some vision going!
-Chris
No comments:
Post a Comment