Ros Tutorials: Failsafe and Control

I'm back!

Today, we are going to look at how to add a failsafe and make a program that communicates with the computer.

First a schematic.

This is the schematic for the failsafe controller. I put this inside a small box with a battery so I could carry it in my hand without cords. The XBee radio inside is paired to a Xbee on the robot, connected to the Serial2 port on the Mega. That step should be fairly simple. Now here is the code for the failsafe controller:

	/*****************************************
	ArduinoFailsafe.ino
	Created September 17, 2016
	Adapted from the State Change Example by Tom Igoe
	which was released into the public domain
	Modified By: thecentralthinkingunit.blogspot.com
	Released into the public domain
	Purpose:
	This program acts as a failsafe for the robot.
	It sends 0xFF to a serial device(Xbee) when the button is pressed.
	It also lights and LED when activated.
	******************************************/
	#include <SoftwareSerial.h>
	SoftwareSerial xbee = SoftwareSerial(2, 3);//Rx, Tx
	// this constant won't change:
	const int buttonPin = 7; // the pin that the pushbutton is attached to
	const int ledPin = 10; // the pin that the LED is attached to
	// Variables will change:
	int buttonPushCounter = 0; // counter for the number of button presses
	int buttonState = 0; // current state of the button
	int lastButtonState = 0; // previous state of the button
	void setup() {
	// initialize the button pin as a input:
	pinMode(buttonPin, INPUT_PULLUP);
	// initialize the LED as an output:
	pinMode(ledPin, OUTPUT);
	// initialize serial communication:
	Serial.begin(9600);
	xbee.begin(9600);
	}
	void loop() {
	// read the pushbutton input pin:
	buttonState = digitalRead(buttonPin);
	if(xbee.available() > 0 && xbee.read() == 0xAA && buttonPushCounter % 2 == 1){
	buttonPushCounter += 1;
	}
	// compare the buttonState to its previous state
	if (buttonState != lastButtonState) {
	// if the state has changed, increment the counter
	if (buttonState == HIGH) {
	// if the current state is HIGH then the button
	// wend from off to on:
	buttonPushCounter++;
	Serial.println("on");
	Serial.print("number of button pushes: ");
	Serial.println(buttonPushCounter);
	} else {
	// if the current state is LOW then the button
	// wend from on to off:
	Serial.println("off");
	}
	// Delay a little bit to avoid bouncing
	delay(50);
	}
	// save the current state as the last state,
	//for next time through the loop
	lastButtonState = buttonState;
	// turns on the LED every other button push by
	// checking the modulo of the button push counter.
	// the modulo function gives you the remainder of
	// the division of two numbers:
	if (buttonPushCounter % 2 == 0) {
	digitalWrite(ledPin, HIGH);
	xbee.write(0xFF);
	delay(50);
	} else {
	digitalWrite(ledPin, LOW);
	}
	}

view raw arduinoFailsafe.ino hosted with ❤ by GitHub

And here is the code for the Mega on-board the robot:

	/*****************************************
	* ArduinoToPython.ino
	* Created September 16, 2016
	* Author: thecentralthinkingunit.blogspot.com
	* Purpose:
	* This program establishes a basic one way serial communications protocol.
	* The computer sends the byte 0x41 to begin a packet.
	* Then it sends two bytes, the first for the linear and the second for the linear. They must have a range from 0x00 to 0xB4(0-180 decimal)
	* There is nothing to end a packet. This may be a good feature to add in the future.
	******************************************/
	//#define DEBUG // Uncomment this line to enable serial debugging of the servo outputs.
	#include <Servo.h>// Import the Servo library
	/* Initialize two constant ints to hold the
	pin numbers for the two channels*/
	const int angularServoPin = 48;//Black wire
	const int linearServoPin = 52;//Blue wire
	Servo angularServo;//Create servo objects for the two channels
	Servo linearServo;
	boolean failsafe = false;//Is the system in failsafe mode?
	byte linear = 0x5A;//Variable to hold the current motor positions.
	byte angular = 0x5A;
	long timeSinceR = 0;
	boolean firstTimeout = true;
	long lastRecFailsafe = 0;
	boolean firstFailsafeDisable = true;//Variable that assists with stopping the motors after leaving failsafe.
	void processIncomingCommands()
	{
	if(Serial.available() > 0)
	{
	byte b = Serial.read();
	if(b == 0x41)//If the received byte is the starting flag.
	{
	delay(25);//Sleep to wait for the next two bytes to get into the buffer.
	linear = Serial.read();
	angular = Serial.read();
	timeSinceR = millis();
	}
	}
	}
	void setup() // This function run once at start
	{
	angularServo.attach(angularServoPin);// associate the servo object with the pin
	linearServo.attach(linearServoPin);
	angularServo.write(90);// set the angle of the servo to 90 degrees
	linearServo.write(90);
	Serial2.begin(9600);
	Serial.begin(9600);
	#ifdef DEBUG
	pinMode(13, OUTPUT);
	#endif
	}
	void loop() //Runs continuously after start
	{
	processIncomingCommands();
	#ifdef DEBUG
	Serial.print("Values: L: ");
	Serial.print(constrain(int(linear), 0, 180));
	Serial.print(" A: ");
	Serial.println(constrain(int(angular), 0, 180));
	#endif
	if(millis() > timeSinceR + 500){
	if(firstTimeout == true){
	Serial2.write(0xAA);
	failsafe = true;
	firstTimeout = false;
	}
	}else{
	firstTimeout = true;
	}
	if(Serial2.available() > 0 && Serial2.read() == 0xFF){
	lastRecFailsafe = millis();
	failsafe = true;
	firstFailsafeDisable = true;
	}
	if(millis() > lastRecFailsafe + 100){
	failsafe = false;
	if(firstFailsafeDisable){
	firstFailsafeDisable = false;
	linear = 0;
	angular = 0;
	}
	}
	if(failsafe){
	linearServo.write(90);
	angularServo.write(90);
	}else{
	linearServo.write(constrain(int(linear), 0, 180));
	angularServo.write(constrain(int(angular), 0, 180));
	}
	}

view raw arduinoToPython.ino hosted with ❤ by GitHub

This sets the failsafe such that, when the failsafe is activated, the led on the controller goes on. The failsafe can be activated one of two ways:
1. The Mega doesn't hasn't received a command in the past 500ms.
2. The button on the controller was pushed.
When the failsafe deactivates, the robot stops, and the LED will go off.

That's all for now!