Thanks
Video coming tonight. I needed to charge my batteries.
Except for a few tweaks, here is the source code.
#include <FiniteStateMachine.h>
#include <NewSoftSerial.h>
#include <Servo2.h>
#include <TimedAction.h>
#include <math.h>
NewSoftSerial mySerial(4, 3); //int rx, int tx)
//config timing
const byte NUMBER_OF_SCANS_PER_SECOND = 2; //change this to suit your needs
const byte SCAN_FREQUENCY = 1000/NUMBER_OF_SCANS_PER_SECOND;
//TimedAction / 'thread' objects
TimedAction scanAction = TimedAction(SCAN_FREQUENCY,scan);
const byte NUMBER_OF_STATES = 3;
int motor_reset = 12; //motor reset pin
int pingPin = 7; //ping sensor pin
int ledpin = 8; //led warning pin
int xPin = 5;
int yPin = 6;
long echo = 0;
int distance = 0; //set distance to zero
long tilt = 0; //set tilt angle to zero
//initialize states
State remote = State(remoteOn);
State automatic = State(autoOn);
State allstop = State(motorstop);
FSM autoStateMachine = FSM(automatic); //start in automatic mode
int switchPin = 11;
Servo myservo; // create servo object to control a servo
int servoPosition = 10; //start position
boolean scanIncrement = true; //increase position?
byte servoIncrementValue = 5;
byte servoDecrementValue = 5;
int Xraw, Yraw;
double xGForce, yGForce, Xangle, Yangle;
int val;
int buttonState;
int mode = 0; //0 = auto mode, 1 = stop mode, 2 = remote mode (yeah, it's confusing but it's my code :)
void setup()
{
pinMode(motor_reset, OUTPUT); //motor reset pin
Serial.begin(9600);
mySerial.begin(9600);
digitalWrite(motor_reset, LOW);
delay(50);
digitalWrite(motor_reset, HIGH);
delay(50);
pinMode(switchPin, INPUT);
buttonState = digitalRead(switchPin);
mySerial.begin(9600); //start software serial communication
myservo.attach(9); // attaches the servo on pin 9 to the servo object
pinMode(yPin, INPUT); //read values from y axis pin
pinMode(xPin, INPUT); //read values from x axis pin
}
void loop () {
val = digitalRead(switchPin);
if (val != buttonState) { // the button state has changed!
if (val == LOW) { // check if the button is pressed
if (mode == 0) { // auto mode on?
mode = 1; //change to stop mode
autoStateMachine.transitionTo(allstop); //all stop
}
else {
if (mode == 1) { // if it is stopped
mode = 2;
autoStateMachine.transitionTo(remote); //change to remote
}
else {
if (mode == 2) { // if it is in remote mode
mode = 0; //auto mode on
autoStateMachine.transitionTo(automatic); //change to auto
}
}
}
}
}
buttonState = val;
//Serial.println(val);
//Serial.println(mode);
autoStateMachine.update();
}
void remoteOn() { //remote stuff - this works
if (Serial.available() > 0) {
int inByte = Serial.read();
switch (inByte) {
case 'w':
motorforward();
break;
case 's':
motorreverse();
break;
case 'd':
rotateccw();
break;
case 'a':
rotatecw();
break;
default:
motorstop();
}
}
}
void autoOn() { //simple auto mode for now
scanAction.check();
}
void scan()
{
scanIncrement ? servoPosition+=servoIncrementValue : servoPosition-=servoDecrementValue; //increment or decrement current position
if (servoPosition > 70){
scanIncrement = false;
servoPosition = 70;
}
else if (servoPosition < 10){
scanIncrement = true;
servoPosition = 10;
}
motorforward();
myservo.write(servoPosition);
long distance = ping();
long tilt = angle();
}
long ping() {
pinMode(pingPin, OUTPUT); // Switch signalpin to output
digitalWrite(pingPin, LOW); // Send low pulse
delayMicroseconds(2); // Wait for 2 microseconds
digitalWrite(pingPin, HIGH); // Send high pulse
delayMicroseconds(5); // Wait for 5 microseconds
digitalWrite(pingPin, LOW); // Holdoff
pinMode(pingPin, INPUT); // Switch signalpin to input
digitalWrite(pingPin, HIGH); // Turn on pullup resistor
echo = pulseIn(pingPin, HIGH); //Listen for echo
distance = (echo / 58.138) * .39; //convert to CM then to inches
//Serial.println(distance);
if (distance <= 3 && distance !=0) //need the !=0 to filter some false zero values
{
avoid();
}
}
//start tilt check
long angle() {
Xraw = pulseIn (xPin, HIGH);
Xraw = pulseIn (xPin, HIGH);
Yraw = pulseIn (yPin, HIGH);
Yraw = pulseIn (yPin, HIGH);
// Calculate G-force in Milli-g's.
xGForce = (( Xraw / 10 ) - 500) * 8;
yGForce = (( Yraw / 10 ) - 500) * 8;
// uncomment next four lines for milli-g output.
//Serial.println(xGForce);
//Serial.print(" ");
//Serial.println(yGForce);
//Serial.print(" ");
// Calculate angle (radians) for both -x and -y axis.
Xangle = asin ( xGForce / 1000.0 );
Yangle = asin ( yGForce / 1000.0 );
// Convert radians to degrees.
Xangle = Xangle * (360 / (2*M_PI));
Yangle = Yangle * (360 / (2*M_PI));
if (Xangle >= 10 || Yangle >= 10 || Yangle <= -10)
{
avoid();
}
//Serial.println(Xangle);
//Serial.print(" ");
//Serial.println(Yangle);
}
//avoid subroutine
void avoid()
{
motorstop();
ledwarning();
delay(1000);
motorreverse();
delay(2000);
motorstop();
delay(1000);
rotateccw();
delay(1000);
motorstop();
delay(1000);
motorforward();
delay(1000);
motorstop();
delay(1000);
rotatecw();
delay(1000);
motorstop();
delay(1000);
}
//fire the led pin as a warning
void ledwarning()
{
digitalWrite(ledpin, HIGH);
delay(1000);
digitalWrite(ledpin, LOW);
}
//subroutine motor forward
void motorforward()
{
//left motor
unsigned char buff1[6];
buff1[0] = 0x80; //start byte - do not change
buff1[1] = 0x00; //device type byte - do not change
buff1[2] = 0x01; //Motore number and direction byte; motor one = 00,01
buff1[3] = 0x7F; //Motor speed "0 to 127" in hex
for(int i=0;i<4;i++) {
mySerial.print(buff1[i], BYTE);
}
//right motor
unsigned char buff2[6];
buff2[0] = 0x80; //start byte - do not change
buff2[1] = 0x00; //device type byte - do not change
buff2[2] = 0x03; //Motore number and direction byte; motor two = 02,03
buff2[3] = 0x7F; //Motor speed "0 to 127" in hex
for(int i=0;i<4;i++) {
mySerial.print(buff2[i], BYTE);
}
}
//subroutine motor reverse
void motorreverse()
{
//left motor
unsigned char buff3[6];
buff3[0] = 0x80; //start byte - do not change
buff3[1] = 0x00; //device type byte - do not change
buff3[2] = 0x00; //Motore number and direction byte; motor one = 00,01
buff3[3] = 0x46; //Motor speed "0 to 127" in hex
for(int i=0;i<4;i++) {
mySerial.print(buff3[i], BYTE);
}
//right motor
unsigned char buff4[6];
buff4[0] = 0x80; //start byte - do not change
buff4[1] = 0x00; //device type byte - do not change
buff4[2] = 0x02; //Motore number and direction byte; motor two = 02,03
buff4[3] = 0x46; //Motor speed "0 to 127" in hex
for(int i=0;i<4;i++) {
mySerial.print(buff4[i], BYTE);
}
//subroutine motor all stop
}
void motorstop()
{
//left motor
unsigned char buff3[6];
buff3[0] = 0x80; //start byte - do not change
buff3[1] = 0x00; //device type byte - do not change
buff3[2] = 0x00; //Motore number and direction byte; motor one = 00,01
buff3[3] = 0x00; //Motor speed "0 to 128" in hex
for(int i=0;i<4;i++) {
mySerial.print(buff3[i], BYTE);
}
//right motor
unsigned char buff4[6];
buff4[0] = 0x80; //start byte - do not change
buff4[1] = 0x00; //device type byte - do not change
buff4[2] = 0x02; //Motore number and direction byte; motor two = 02,03
buff4[3] = 0x00; //Motor speed "0 to 128" in hex
for(int i=0;i<4;i++) {
mySerial.print(buff4[i], BYTE);
}
}
void rotateccw()
{
//left motor
unsigned char buff3[6];
buff3[0] = 0x80; //start byte - do not change
buff3[1] = 0x00; //device type byte - do not change
buff3[2] = 0x01; //Motore number and direction byte; motor one = 00,01
buff3[3] = 0x40; //Motor speed "0 to 127" in hex
for(int i=0;i<4;i++) {
mySerial.print(buff3[i], BYTE);
}
//right motor
unsigned char buff4[6];
buff4[0] = 0x80; //start byte - do not change
buff4[1] = 0x00; //device type byte - do not change
buff4[2] = 0x02; //Motore number and direction byte; motor two = 02,03
buff4[3] = 0x40; //Motor speed "0 to 127" in hex
for(int i=0;i<4;i++) {
mySerial.print(buff4[i], BYTE);
}
}
void rotatecw()
{
//left motor
unsigned char buff3[6];
buff3[0] = 0x80; //start byte - do not change
buff3[1] = 0x00; //device type byte - do not change
buff3[2] = 0x00; //Motore number and direction byte; motor one = 00,01
buff3[3] = 0x40; //Motor speed "0 to 127" in hex
for(int i=0;i<4;i++) {
mySerial.print(buff3[i], BYTE);
}
//right motor
unsigned char buff4[6];
buff4[0] = 0x80; //start byte - do not change
buff4[1] = 0x00; //device type byte - do not change
buff4[2] = 0x03; //Motore number and direction byte; motor two = 02,03
buff4[3] = 0x40; //Motor speed "0 to 127" in hex
for(int i=0;i<4;i++) {
mySerial.print(buff4[i], BYTE);
}
}
