/*
Software Requirements:
----------------------
-Arduino IDE 1.0
Hardware Requirements:
----------------------
-Arduino
-Pressure sensors
-Gyroscope
-Servo motors
Project Requirents:
-------------------
• Game pad controller
*/
#include <SPI.h>
#include "mcp4261.h"
#include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"
#define SensorV 15
#define Button 4
const int POT_0_CS = 6;
const int LOOP_DELAY = 200;
RF24 radio(9,10);
MCP4261 pot0 = MCP4261(POT_0_CS);
const uint64_t pipes[6] = {0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL, 0xF0F0F0F0E2LL, 0xF0F0F0F0D3LL, 0xF0F0F0F0E3LL, 0xF0F0F0F0D4LL};
int speed = 0;
int angle = 0;
int y1_axis = 0, y2_axis = 0, y3_axis_1, y3_axis_2;
int y1_last = 0, y2_last = 0;
int y1_thres = 0, y2_thres = 0 , y3_thres_1 , y3_thres_2;
int sensorBL;
int sensorBR;
int sensorSL;
int sensorSR;
int prev_y1_axis = 0;
int y1_hold = 0;
unsigned long timer;
int start = 0;
byte step = 0;
String steps , laststep;
int freq;
int runC;
int press1 = 0 , press2 = 0;
byte pos;
int pox;
void setup() {
Serial.begin(115200);
pinMode(POT_0_CS , OUTPUT);
pinMode(10 , OUTPUT);
selectSlave(1);
radio.begin();
radio.setRetries(15, 15);
pinMode(Button , OUTPUT);
digitalWrite(Button , LOW);
selectSlave(2);
// Setup SPI communications
SPI.setDataMode(SPI_MODE0);
SPI.setBitOrder(MSBFIRST);
SPI.setClockDivider(SPI_CLOCK_DIV8);
SPI.begin();
pot0.initialize();
}
void loop() {
receiveData();
checkV();
if (sensorSL > 120) {
speed = map( sensorSL , 0 , 1023 , 0 , 255);
if (sensorSL < 700) {
selectSlave(2);
pot0.setWiper0(86);
press1 = 0;
}
else {
selectSlave(2);
pot0.setWiper0(0);
press1 = 1;
}
}
else if (sensorSR > 120) {
speed = map( sensorSR , 0 , 1023 , 0 , 255);
if (sensorSR < 700) {
selectSlave(2);
pot0.setWiper0(180);
press1 = 0;
}
else {
selectSlave(2);
pot0.setWiper0(256);
press1 = 1;
}
}
if (sensorSL < 120 && sensorSR < 120 && pos == 0) {
selectSlave(2);
pot0.setWiper0(127);
press1 = 0;
}
else if (sensorSL < 120 && sensorSR < 120 && pos == 1) { //left down
selectSlave(2);
pot0.setWiper0(256);
pot0.setWiper1(256);
}
else if (sensorSL < 120 && sensorSR < 120 && pos == 2) { //left up
selectSlave(2);
pot0.setWiper0(256);
pot0.setWiper1(0);
}
else if (sensorSL < 120 && sensorSR < 120 && pos == 3) { //right down
selectSlave(2);
pot0.setWiper0(0);
pot0.setWiper1(256);
}
else if (sensorSL < 120 && sensorSR < 120 && pos == 4) { //right up
selectSlave(2);
pot0.setWiper0(0);
pot0.setWiper1(0);
}
else if (sensorSL < 120 && sensorSR < 120 && pos == 5) { //left
selectSlave(2);
pot0.setWiper0(256);
pot0.setWiper1(127);
press1 = 1;
}
else if (sensorSL < 120 && sensorSR < 120 && pos == 6) { //right
selectSlave(2);
pot0.setWiper0(0);
pot0.setWiper1(127);
press1 = 1;
}
else if (sensorSL < 120 && sensorSR < 120 && pos == 7) { //down
selectSlave(2);
pot0.setWiper0(127);
pot0.setWiper1(256);
press1 = 1;
}
else if (sensorSL < 120 && sensorSR < 120 && pos == 8) { //up
selectSlave(2);
pot0.setWiper0(127);
pot0.setWiper1(0);
press1 = 1;
}
/*else if (sensorSL < 120 && sensorSR < 120 && pos == 9) { //Jump
selectSlave(2);
pot0.setWiper0(127);
pot0.setWiper1(256);
press1 = 1;
}
else if (sensorSL < 120 && sensorSR < 120 && pos == 10) { //Crawl
selectSlave(2);
pot0.setWiper0(127);
pot0.setWiper1(0);
press1 = 1;
}*/
//0 right - 64 centr - 127 left
if (sensorBL > 250 && pos == 0) {
speed = map( sensorBL , 0 , 1023 , 0 , 255);
if (sensorBL < 500) { //less pressure 90+20=110
selectSlave(2);
pot0.setWiper1(50);
angle = 95;
press2 = 0;
}
else if (sensorBL > 500 && pos == 0) { //more pressre 90+90 = 180
selectSlave(2);
pot0.setWiper1(0);
angle = 105;
press2 = 1;
}
}
else if (sensorBR > 250 && pos == 0) {
speed = map( sensorBR , 0 , 1023 , 0 , 255);
if (sensorBR < 500) { //less pressure
selectSlave(2);
pot0.setWiper1(50);
angle = 95;
press2 = 0;
}
else if (sensorBR > 500 && pos == 0) { //more pressre
selectSlave(2);
pot0.setWiper1(0);
angle = 105;
press2 = 1;
}
}
else if (pos == 0 && (start == 0 || millis() - timer > 800) && y1_axis > y1_thres - SensorV && y1_axis < y1_thres + SensorV && y2_axis > y2_thres - SensorV && y2_axis < y2_thres + SensorV && sensorBL < 200 && sensorBR < 200) //flat dead end no servo movement - center position - can we separate on different lines the pressure sensor and accell so that I can set the dead end value separately for the press sens and acc ?
{
if (angle == 70 && runC > 0) {
angle = 78;
selectSlave(2);
pot0.setWiper1(150);
runC--;
press2 = 0;
}
else {
selectSlave(2);
pot0.setWiper1(128);
start = 1;
angle = 87;
press2 = 0;
}
//Serial.println("Servo goes to zero");
step = 0;
steps = "0";
}
else if ((y1_axis < y1_thres - SensorV) && (step == 0 || step == 2))
{
step = 1;
steps = "1";
}
else if ((y2_axis < y2_thres - SensorV) && (step == 0 || step == 1))
{
step = 2;
steps = "2";
}
if (!steps.equals(laststep) && pos == 0) {
if ((step == 1 || step == 2) && millis() - timer < 300) {
if (freq > 1) {
selectSlave(2);
pot0.setWiper1(256);
angle = 70;
press2 = 1;
}
else {
selectSlave(2);
pot0.setWiper1(150);
angle = 78;
press2 = 0;
}
freq++;
runC = 1;
}
else if ((step == 1 || step == 2 && millis() - timer > 675)) {
selectSlave(2);
pot0.setWiper1(150);
angle = 78;
freq = 1;
press2 = 0;
}
timer = millis();
}
if (press1 == 1 || press2 == 1)
digitalWrite(Button , LOW);
else
digitalWrite(Button , HIGH);
/* Serial.print("SL: " + String(sensorSL) + " SR: " + String(sensorSR));
Serial.print(" BL: " + String(sensorBL) + " BR: " + String(sensorBR));
Serial.print(" Y1: " + String(y1_axis) + " Y2: " + String(y2_axis));
Serial.print("Pos: " + String(pos));
Serial.print(" Y3x: " + String(y3_axis_1) + " Y3y: " + String(y3_axis_2));
Serial.println(" Y3xx: " + String(y3_thres_1) + " Y3yy: " + String(y3_thres_2));*/
Serial.println("Angle: " + String(angle));
laststep = steps;
delay(80);
}
void receiveData() {
selectSlave(1);
for (int i = 0 ; i < 3;) {
radio.openReadingPipe(1, pipes[i]);
radio.startListening();
if ( radio.available() )
{
char rawMessage[40] = {0} ;
while ( radio.available() )
{
radio.read( &rawMessage, sizeof(rawMessage) );
// Serial.println(rawMessage);
delay(10);
}
radio.stopListening();
String data = String(rawMessage);
switch (i) {
case 0:
y1_thres = data.substring(0, data.indexOf(",")).toInt();
data.remove(0, data.indexOf(",") + 1);
y1_axis = data.substring(0, data.indexOf(",")).toInt();
data.remove(0, data.indexOf(",") + 1);
sensorBR = data.substring(0, data.indexOf(",")).toInt();
data.remove(0, data.indexOf(",") + 1);
sensorSR = data.substring(0, data.length()).toInt();
break;
case 1:
y2_thres = data.substring(0, data.indexOf(",")).toInt();
data.remove(0, data.indexOf(",") + 1);
y2_axis = data.substring(0, data.indexOf(",")).toInt();
data.remove(0, data.indexOf(",") + 1);
sensorBL = data.substring(0, data.indexOf(",")).toInt();
data.remove(0, data.indexOf(",") + 1);
sensorSL = data.substring(0, data.length()).toInt();
break;
case 2:
y3_axis_1 = data.substring(0, data.indexOf(",")).toInt();
data.remove(0, data.indexOf(",") + 1);
y3_axis_2 = data.substring(0, data.indexOf(",")).toInt();
data.remove(0, data.indexOf(",") + 1);
y3_thres_1 = data.substring(0, data.indexOf(",")).toInt();
data.remove(0, data.indexOf(",") + 1);
y3_thres_2 = data.substring(0, data.indexOf(",")).toInt();
data.remove(0, data.indexOf(",") + 1);
pox = data.substring(0, data.length()).toInt();
break;
}
i = i + 1;
}
}
radio.stopListening();
radio.openWritingPipe(pipes[3]);
String t = String(pox);
char callback[5];
t.toCharArray(callback , t.length() + 1);
radio.write( &callback, sizeof(callback));
}
void checkV() {
if (y3_axis_1 > y3_thres_1 - 15 && y3_axis_1 < y3_thres_1 + 15 && y3_axis_2 > y3_thres_2 - 15 && y3_axis_2 < y3_thres_2 + 15)
pos = 0; //center
else if (y3_axis_1 > 0) { //left
if (y3_axis_2 > y3_thres_2 - 15 && y3_axis_2 < y3_thres_2 + 15)
pos = 5; //left
else if (y3_axis_2 > 0) {
if (y3_axis_1 > y3_thres_1 - 15 && y3_axis_1 < y3_thres_1 + 15)
pos = 7; //Down
else
pos = 1; //left down
}
else if (y3_axis_2 < 0) {
if (y3_axis_1 > y3_thres_1 - 15 && y3_axis_1 < y3_thres_1 + 15)
pos = 8; //UP
else
pos = 2; //left up
}
}
else if (y3_axis_1 < 0) { //right
if (y3_axis_2 > y3_thres_2 - 15 && y3_axis_2 < y3_thres_2 + 15)
pos = 6; //Right
else if (y3_axis_2 > 0) {
if (y3_axis_1 > y3_thres_1 - 15 && y3_axis_1 < y3_thres_1 + 15)
pos = 7; //Down
else
pos = 3; //right down
}
else if (y3_axis_2 < 0) {
if (y3_axis_1 > y3_thres_1 - 15 && y3_axis_1 < y3_thres_1 + 15)
pos = 8; //UP
else
pos = 4; //right up
}
}
}
void selectSlave(byte x) {
switch (x) {
case 1:
digitalWrite(POT_0_CS , HIGH);
digitalWrite(10 , LOW);
break;
case 2:
digitalWrite(POT_0_CS , LOW);
digitalWrite(10 , HIGH);
break;
}
}