//TankBot - A physical/virtual game
//version 0.2 2012/03/29
//Joe McManus josephmc@cmu.edu
//Copyright (C) 2012 Joe McManus
//This program is free software: you can redistribute it and/or modify
//it under the terms of the GNU General Public License as published by
//the Free Software Foundation, either version 3 of the License, or
//(at your option) any later version.
//
//This program is distributed in the hope that it will be useful,
//but WITHOUT ANY WARRANTY; without even the implied warranty of
//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
//GNU General Public License for more details.
//
//You should have received a copy of the GNU General Public License
//along with this program. If not, see .
#include
#include
#include
#include "WiFlySerial.h"
#include "Credentials.h"
#include "Servo.h"
//A=Motor A, B=Motor B
#define dirA 12
#define dirB 13
#define brakeA 9
#define brakeB 8
#define motorA 3
#define motorB 11
#define sensA 14
#define sensB 15
#define laser 5
#define pingPin 10
Servo turret; // We call the servo turret, for the tank
// 2 - receive TX (Send from Wifly, Receive to Arduino)
// 3 - send RX (Send from Arduino, Receive to WiFly)
WiFlySerial WiFly(7,6);
#define REQUEST_BUFFER_SIZE 120
#define HEADER_BUFFER_SIZE 150
#define BODY_BUFFER_SIZE 100
char bufRequest[REQUEST_BUFFER_SIZE];
char bufHeader[HEADER_BUFFER_SIZE];
char bufBody[BODY_BUFFER_SIZE];
char netInput; //Temp spot for reading network input
char netDirection; //Store the direction
int netDistance; //Store the distance
int pingVal; //Hold the value from the ping
int ampA; //Hold the value from the ampA monitor
int ampB; //Hold the value from the ampB monitor
int servoPosition; //Hold the servo position
int defSpeed = 145; //Default speed for motors
unsigned int duration, inches; //Hold vals for inside the ping test
void setup() {
Serial.begin(9600);
pinMode(dirA, OUTPUT);
pinMode(dirB, OUTPUT);
pinMode(brakeA, OUTPUT);
pinMode(brakeB, OUTPUT);
pinMode(motorA, OUTPUT);
pinMode(motorB, OUTPUT);
pinMode(pingPin, OUTPUT);
pinMode(sensA, INPUT);
pinMode(sensB, INPUT);
pinMode(laser, OUTPUT);
laserSetup(); //blink the laser
wiFlySetup(); //Connect to the network.
servoSetup(); //Pan the servo
motorCheck(); //Just Quickly Pulse the motors
}
void servoSetup() { // Set up the servo and test it.
turret.attach(4); //Attach the servo
for (int i=1; i <180; i++) { //Pan L-> R
turret.write(i);
delay(10);
}
for (int i=179; i >0; i--) { //Pan R-> L
turret.write(i);
delay(10);
}
turret.write(100); //Sit in the middle
}
void laserSetup() { //Just blink the laser for effect
for (int i=1; i <5; i++) {
digitalWrite(laser, HIGH);
delay(250);
digitalWrite(laser, LOW);
}
}
void wiFlySetup() { //Connect to the WiFly
Serial.println(F("Starting WiFly" ) );
WiFly.begin();
Serial << F("Starting WiFly.") << endl;
// get MAC
Serial << F("MAC: ") << WiFly.getMAC(bufRequest, REQUEST_BUFFER_SIZE) << endl;
// is connected ?
WiFly.setDebugChannel( (Print*) &Serial);
WiFly.setAuthMode( WIFLY_AUTH_WPA2_PSK);
WiFly.setJoinMode( WIFLY_JOIN_AUTO );
WiFly.setDHCPMode( WIFLY_DHCP_ON );
// if not connected restart link
WiFly.getDeviceStatus();
if (! WiFly.isifUp() ) {
Serial << "Leave:" << ssid << WiFly.leave() << endl;
// join
if (WiFly.setSSID(ssid) ) {
Serial << "SSID Set :" << ssid << endl;
}
if (WiFly.setPassphrase(passphrase)) {
Serial << "Passphrase Set :" << endl;
}
Serial << "Joining... :"<< ssid << endl;
if ( WiFly.join() ) {
Serial << F("Joined ") << ssid << F(" successfully.") << endl;
WiFly.setNTP( ntp_server ); // use your favorite NTP server
}
else {
Serial << F("Join to ") << ssid << F(" failed.") << endl;
}
} // if not connected
Serial << F("IP: ") << WiFly.getIP(bufRequest, REQUEST_BUFFER_SIZE) << endl <<
F("Netmask: ") << WiFly.getNetMask(bufRequest, REQUEST_BUFFER_SIZE) << endl <<
F("Gateway: ") << WiFly.getGateway(bufRequest, REQUEST_BUFFER_SIZE) << endl <<
F("DNS: ") << WiFly.getDNS(bufRequest, REQUEST_BUFFER_SIZE) << endl ;
memset (bufBody,'\0',BODY_BUFFER_SIZE);
WiFly.SendCommand("show q 0x177 ",">", bufBody, BODY_BUFFER_SIZE);
Serial << F("WiFly Sensors: ") << bufBody << endl;
WiFly.SendCommand("show q t ",">", bufBody, BODY_BUFFER_SIZE);
Serial << F("WiFly Temp: ") << bufBody << endl;
Serial << F("WiFly battery: ") << WiFly.getBattery(bufBody, BODY_BUFFER_SIZE) << endl;
WiFly.SendCommand("set comm remote 0",">", bufBody, BODY_BUFFER_SIZE);
WiFly.closeConnection();
//Serial << F("WiFly now listening for commands. Type 'exit' to listen for wifi traffic. $$$ (no CR) for command-mode.") << endl;
// clear out prior requests.
WiFly.uart.flush();
while (WiFly.uart.available() )
WiFly.uart.read();
}
int pingRead() {
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW); // Ensure pin is low
delayMicroseconds(2); //Brief Pause
digitalWrite(pingPin, HIGH); // Start ranging
delayMicroseconds(5); // with 5 microsecond burst
digitalWrite(pingPin, LOW); // End ranging
pinMode(pingPin, INPUT); // Set pin to INPUT
duration = pulseIn(pingPin, HIGH); // Read echo pulse
inches = duration / 74 / 2; // Convert to inches
return inches; // Display result
}
void obstacleCheck() { //Check for obstacles
pingVal=pingRead();
if (pingVal <= 12) { // an obstacle within 12inches
emergencyStop();
delay(50);
//Put Motors in reverse.
digitalWrite(dirA, LOW);
digitalWrite(dirB, LOW);
//Go in reverse for .25 seconds
analogWrite(motorA, 128);
analogWrite(motorB, 128);
delay(250);
//Go back to the left for .25 seconds
analogWrite(motorA, 128);
analogWrite(motorB, 64);
delay(250);
ampCheck();
emergencyStop(); //We've backed up, now stop for a moment
delay(10);
}
}
void emergencyStop() {
digitalWrite(brakeA, HIGH);
digitalWrite(brakeB, HIGH);
analogWrite(motorA, 0);
analogWrite(motorB, 0);
digitalWrite(brakeA, LOW);
digitalWrite(brakeB, LOW);
}
void ampCheck() { //Check to see how much power we are pulling, shut down if we pull more than 2A
ampA=0;
ampB=0;
for (int i=0; i < 25; i++) {
ampA=ampA + analogRead(sensA);
ampB=ampB + analogRead(sensB);
delay(5);
}
ampA=(ampA/25);
ampB=(ampB/25);
if (ampA > 1000 || ampB > 1000) {
analogWrite(motorA, 0);
analogWrite(motorB, 0);
delay(1000);
}
}
void randomPosition(int netDistance) { //Randomly run somewhere
int j=0; //We'll use this to change direction every 5 turns
int directions[] = { 'F', 'B', 'L', 'R' }; // possible direction;
int r=random(0,3); //pick a random direction;
for (int i=0; i < netDistance; i++) {
drive(directions[r], i);
delay(10);
if (j != 5) {
j++;
}
else {
r=random(0,3);
j=0;
}
}
}
void motorCheck() {
drive('F', 1);
emergencyStop();
}
void drive(char direct, int distance) {
switch (direct) {
case 'F': //Forward
for(int i=0; i< distance; i++) {
digitalWrite(dirA, HIGH);
digitalWrite(dirB, HIGH);
analogWrite(motorA, defSpeed);
analogWrite(motorB, defSpeed);
obstacleCheck();
ampCheck();
delay(100);
}
break;
case 'B': //back
emergencyStop();
for(int i=0; i< distance; i++) {
digitalWrite(dirA, LOW);
digitalWrite(dirB, LOW);
analogWrite(motorA, defSpeed);
analogWrite(motorB, defSpeed);
ampCheck();
delay(100);
}
break;
case 'L':
for(int i=0; i< distance; i++) {
digitalWrite(dirA, HIGH);
digitalWrite(dirB, HIGH);
analogWrite(motorA, 64);
analogWrite(motorB, 160);
obstacleCheck();
ampCheck();
delay(100);
}
break;
case 'R':
for(int i=0; i< distance; i++) {
digitalWrite(dirA, HIGH);
digitalWrite(dirB, HIGH);
analogWrite(motorA, 160);
analogWrite(motorB, 64);
obstacleCheck();
ampCheck();
delay(100);
}
break;
case 'N':
randomPosition(distance);
break;
case 'Z': //Control the lazer
digitalWrite(laser, HIGH);
servoPosition = turret.read(); //Read the current servo position
if (distance == 1) { //Fire once
delay(5000);
}
else if (distance == 2) { //Fire 30deg
for (int i=servoPosition; i < (servoPosition + 15); i++) {
//if (i > 179 ) { break;}
turret.write(i);
delay(500);
}
for (int i=servoPosition; i > (servoPosition - 15); i--) {
//if (i < 1 ) { break;}
turret.write(i);
delay(500);
}
}
else {
for (int i=2; i <178; i++) { //Pan L-> R
turret.write(i);
delay(50);
}
for (int i=178; i > 2; i--) { //Pan R-> L
turret.write(i);
delay(50);
}
}
turret.write(servoPosition); //Go back to initial pos
digitalWrite(laser, LOW);
break;
case 'S':
turret.write(distance); //Move the turret/servo
break;
case 'X': //Stop!
analogWrite(motorA, 0);
analogWrite(motorB, 0);
break;
}
}
void loop() {
while(WiFly.uart.available() > 0) {
netInput = WiFly.uart.read(); //use parseInt to look for the next int
//Serial.println(netInput);
if(netInput == '#') { //All commands start with a #
netDirection= WiFly.uart.read(); //The direction has to be a Char F,B,L,R,N,Z,S
netDistance = WiFly.uart.parseInt();
//Serial.print("Direction: ");
//Serial.println(netDirection);
//Serial.print("Distance : ");
//Serial.println(netDistance);
drive(netDirection, netDistance);
// Stop the motors
emergencyStop();
//drive('Z', 1);
}
}
delay(100);
}