@ScruffR @kennethlimcp
Thanks both of you for your suggestions! Here is my code although it is a little long. If you see that I have a servo interrupt that’s for a hardware thing not related to this thread… Sorry it is a little long the solar panel thing is about half way through but I bolded it. Also, if you see any ways to possibly make the delays that might be bogging it up go away that would be greatly appreciated as well!
include <Servo.h>
include <Wire.h>
include <Adafruit_NeoPixel.h>
Servo solarmotor;
Servo clawmotor;
int fval = 0;
int pin = A5;// the fsr for rover 3
const int solarInterrupt = 33;
const int clawInterrupt = 43;
int leftDistance, rightDistance;
int forwardLeftDistance, forwardRightDistance;
const int dangerThresh = 10;
int count = 0;
int frontLightPin = A0;
int backLightPin = A1;
int frontLight = 0;
int backLight = 0;
int light = 0;
define PIN 22//for the neopixel
define NUMPIXELS 12
//Sonar 1
int echoPin1 =7;
int initPin1 =8;
int distance1 = 0;
//Sonar 2
int echoPin2 = 5;
int initPin2 =6;
int distance2 = 0;
//Sonar 3
int echoPin3 = 4;
int initPin3 = 11;
int distance3 = 0;
//Sonar 4
int echoPin4 =31;
int initPin4 =30;
int distance4 =0;
const int awarenessPin = 35;
//m1
const int inA1 = 24;
const int inA2 = 26;
int speedPinA = 9;
//m2
const int inB1 = 25;
const int inB2 = 27;
int speedPinB = 10;
int LED = 13;
int x = 0;
const int solenoidPin = 33;
const int xPin = 2; // X output of the accelerometer
const int yPin = 3; // Y output of the accelerometer
Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NUMPIXELS, PIN, NEO_GRB);
int delayval = 500; // delay for half a second
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
Wire.begin(8); // for i2c
Wire.onReceive(receiveEvent);
pixels.begin();
pinMode(initPin1, OUTPUT);// for the ultrasounds
pinMode(echoPin1, INPUT);
pinMode(initPin2, OUTPUT);
pinMode(echoPin2, INPUT);
pinMode(initPin3, OUTPUT);
pinMode(echoPin3, INPUT);
pinMode(initPin4, OUTPUT);
pinMode(echoPin4, INPUT);
pinMode(inA1,OUTPUT);
pinMode(inA2,OUTPUT);
pinMode(speedPinA,OUTPUT);
pinMode(inB1,OUTPUT);
pinMode(inB2,OUTPUT);
pinMode(speedPinB,OUTPUT);
pinMode(xPin, INPUT);
pinMode(yPin, INPUT);
pinMode(solenoidPin, OUTPUT);
digitalWrite(solenoidPin, LOW);
pinMode(awarenessPin, OUTPUT);
solarmotor.attach(32);
clawmotor.attach(42);
pinMode(solarInterrupt, OUTPUT);
pinMode(clawInterrupt, OUTPUT);
digitalWrite(clawInterrupt, LOW);
}
void receiveEvent(int bytes) {
x = Wire.read();
Serial.print(x);// read one character from the I2C
}
void loop() {
// put your main code here, to run repeatedly:
// for the motor drive
count = count + 1;
fval = analogRead(pin);
distance1 = getDistance(initPin1, echoPin1);
//printDistance(1, distance1);
delay(150);
/*
if(distance1 > 20){
digitalWrite(awarenessPin, HIGH);
}
else{
digitalWrite(awarenessPin, LOW);
}
*/
distance2 = getDistance(initPin2, echoPin2);
//printDistance(2, distance2);
delay(150);
distance3 = getDistance(initPin3, echoPin3);
// printDistance(3, distance3);
delay(150);
distance4 = getDistance(initPin4, echoPin4);
//printDistance(4, distance4);
delay(500);
rightDistance = distance1;
leftDistance = distance2;
forwardRightDistance = distance3;
forwardLeftDistance = distance4;
// for the accelerometer now
int pulseX, pulseY;
// variables to contain the resulting accelerations
int accelerationX, accelerationY;
// read pulse from x- and y-axes:
pulseX = pulseIn(xPin, HIGH);
pulseY = pulseIn(yPin, HIGH);
// convert the pulse width into acceleration
// accelerationX and accelerationY are in milli-g's:
// earth's gravity is 1000 milli-g's, or 1g.
accelerationX = ((pulseX / 10) - 500) * 8;
accelerationY = ((pulseY / 10) - 500) * 8;
if(accelerationX <= -80 && accelerationX >= -450){
digitalWrite(awarenessPin, HIGH);
}
else{
digitalWrite(awarenessPin, LOW);
}
// for the communication between electron and arduino mega
if (x == '0') {
digitalWrite(LED, HIGH);
delay(200);
digitalWrite(LED, LOW);
delay(200);
Serial.print("Hello");
forward();
if (forwardRightDistance < 7 && forwardLeftDistance < 7){
delay(2000);
compareDistance();
}
else{
forward();
}
if(accelerationX >= -40 && accelerationX <= -300){
digitalWrite(inA1, LOW);
digitalWrite(inA2, HIGH);
digitalWrite(inB1, LOW);
digitalWrite(inB2, HIGH);
analogWrite(speedPinA, 125);
analogWrite(speedPinB, 125);
}
else{
digitalWrite(inA1, LOW);
digitalWrite(inA2, HIGH);
digitalWrite(inB1, LOW);
digitalWrite(inB2, HIGH);
analogWrite(speedPinA, 255);
analogWrite(speedPinB, 255);
}
}
if (x == '1') {
digitalWrite(LED, HIGH);
delay(200);
digitalWrite(LED, LOW);
delay(200);
Serial.print("hi");
backWard();
}
if (x == '2') {
digitalWrite(LED, HIGH);
delay(200);
digitalWrite(LED, LOW);
delay(200);
Serial.print("today");
sTop();
}
//If value received is 3 blink LED for 400 ms
if (x == '3') {
digitalWrite(LED, HIGH);
delay(400);
digitalWrite(LED, LOW);
delay(400);
turnLeft();
delay(1000);
sTop();
delay(6000);
if(leftDistance < 6){
sTop();
delay(100);
if(rightDistance > 6){
turnRight();
delay(500);
sTop();
int distanceFwd = leftDistance + rightDistance;
if (distanceFwd > dangerThresh) //if path is clear
{
forward(); //move forward
}
else{
delay(4000);
}
}
}
}
if (x == '4') {
digitalWrite(LED, HIGH);
delay(200);
digitalWrite(LED, LOW);
delay(200);
turnRight();
delay(1000);
sTop();
delay(6000);
if(rightDistance < 6){
sTop();
delay(100);
if(leftDistance > 6){
turnLeft();
delay(500);
sTop();
delay(300);
int distanceFwd = leftDistance && rightDistance;
if (distanceFwd > dangerThresh) //if path is clear
{
forward(); //move forward
}
else{
delay(4000);
}
}
}
}
if (x == '5') {
digitalWrite(LED, HIGH);
delay(200);
digitalWrite(LED, LOW);
delay(200);
turnRight();
delay(500);
sTop();
delay(6000);
if(rightDistance < 6){
sTop();
delay(100);
if(leftDistance > 6){
turnLeft();
delay(500);
sTop();
delay(300);
int distanceFwd = forwardLeftDistance + forwardRightDistance;
if (distanceFwd > dangerThresh) //if path is clear
{
forward(); //move forward
}
else{
delay(4000);
}
}
}
}
if (x == '6') {
digitalWrite(LED, HIGH);
delay(400);
digitalWrite(LED, LOW);
delay(400);
turnLeft();
delay(500);
sTop();
delay(6000);
if(leftDistance < 6){
sTop();
delay(100);
if(rightDistance > 6){
turnRight();
delay(500);
sTop();
if (forwardRightDistance > 6 && forwardLeftDistance > 6) //if path is clear
{
forward(); //move forward
}
else{
delay(4000);
}
}
}
}
if(x == '7'){
if (forwardRightDistance > 6 && forwardLeftDistance > 6 || forwardRightDistance <= 0 || forwardLeftDistance <= 0){
forward();
if(accelerationX <= -40 && accelerationX >= -450){
digitalWrite(inA1, LOW);
digitalWrite(inA2, HIGH);
digitalWrite(inB1, LOW);
digitalWrite(inB2, HIGH);
analogWrite(speedPinA, 125);
analogWrite(speedPinB, 125);
digitalWrite(awarenessPin, HIGH);
}
else{
digitalWrite(inA1, LOW);
digitalWrite(inA2, HIGH);
digitalWrite(inB1, LOW);
digitalWrite(inB2, HIGH);
analogWrite(speedPinA, 255);
analogWrite(speedPinB, 255);
digitalWrite(awarenessPin, LOW);
}
}
//if(forwardRightDistance <= 0){
// forward();
// }
//if(forwardLeftDistance <= 0){
// forward();
//}
else{
sTop();
delay(1000);
backWard();
delay(1500);
compareDistance();
}
if(x == '8'){
int tick = 0;
tick = tick + 1;
clawmotor.write(90);
while(tick == 10000){
clawmotor.write(180);
}
}
/*
if(forwardRightDistance < 7 && forwardLeftDistance > 7){// if right side is greater that the left
sTop();
delay(600);
if(leftDistance > 15){
turnLeft();
}
if(rightDistance > 15){
turnRight();
}
if(rightDistance > 15 && leftDistance > 15){
compareDistance();
}
}
if(forwardRightDistance > 7 && forwardLeftDistance < 7){
sTop();
delay(600);
if(leftDistance > 15){
turnLeft();
}
if(rightDistance > 15){
turnRight();
}
}
else{
compareDistance();
}
*/
}
**frontLight = analogRead(frontLightPin);**
> backLight = analogRead(backLightPin);**
> //Serial.print("back");**
> //Serial.print(backLight);**
> // Serial.print("front");**
> //Serial.print(frontLight);**
> if(frontLight > backLight ){**
> delay(100);**
> digitalWrite(solarInterrupt, HIGH);**
> delay(200);**
> solarmotor.write(30);**
> digitalWrite(solarInterrupt, LOW);**
> }**
> if(frontLight < backLight ){**
> delay(100);**
> digitalWrite(solarInterrupt, HIGH);**
> delay(200);**
> solarmotor.write(80);**
> digitalWrite(solarInterrupt, LOW);**
}
/*
if(frontLight > backLight && frontLight > 1000){
delay(100);
solarmotor.write(80);
}
if(frontLight < backLight && backLight > 1000){
delay(100);
solarmotor.write(0);
}
*/
Serial.print(fval);
delay(100);
Serial.print(",");
if(fval < 80){
clawmotor.write(180);
delay(500);
}
if(fval > 81){
clawmotor.write(90);
delay(500);
}
if (forwardRightDistance > 6 && forwardLeftDistance > 6 || forwardRightDistance <= 0 || forwardLeftDistance <= 0){
pixels.setPixelColor(1, pixels.Color(0, 255, 0));
pixels.setPixelColor(2, pixels.Color(0, 255, 0));
pixels.setPixelColor(3, pixels.Color(0, 255, 0));
pixels.setPixelColor(4, pixels.Color(0, 255, 0));
pixels.setPixelColor(5, pixels.Color(0, 255, 0));
pixels.setPixelColor(6, pixels.Color(0, 255, 0));
pixels.setPixelColor(7, pixels.Color(0, 255, 0));
pixels.setPixelColor(8, pixels.Color(0, 255, 0));
pixels.setPixelColor(9, pixels.Color(0, 255, 0));
pixels.setPixelColor(10, pixels.Color(0, 255, 0));
pixels.setPixelColor(11, pixels.Color(0, 255, 0));
pixels.setPixelColor(12, pixels.Color(0, 255, 0));
}
else{
pixels.setPixelColor(1, pixels.Color(255, 0, 0));
pixels.setPixelColor(2, pixels.Color(255, 0, 0));
pixels.setPixelColor(3, pixels.Color(255, 0, 0));
pixels.setPixelColor(4, pixels.Color(255, 0, 0));
pixels.setPixelColor(5, pixels.Color(255, 0, 0));
pixels.setPixelColor(6, pixels.Color(255, 0, 0));
pixels.setPixelColor(7, pixels.Color(255, 0, 0));
pixels.setPixelColor(8, pixels.Color(255, 0, 0));
pixels.setPixelColor(9, pixels.Color(255, 0, 0));
pixels.setPixelColor(10, pixels.Color(255, 0, 0));
pixels.setPixelColor(11, pixels.Color(255, 0, 0));
pixels.setPixelColor(12, pixels.Color(255, 0, 0));
}
// pixels.Color takes RGB values, from 0,0,0 up to 255,255,255
// pixels.setPixelColor(i, pixels.Color(0,150,0)); // Moderately bright green color.
pixels.show(); // This sends the updated pixel color to the hardware.
}//WHERE VOID LOOP ENDS!!!`````````````````````````````````````````````````````
int getDistance (int initPin, int echoPin){
digitalWrite(initPin, HIGH);
delayMicroseconds(10);
digitalWrite(initPin, LOW);
unsigned long pulseTime = pulseIn(echoPin, HIGH);
int distance = pulseTime/58;
return distance;
}
///*
void printDistance(int id, int dist){
///*
Serial.print(id);
for (int i = 0; i <= dist; i++) {
Serial.print("-");
}
Serial.print(dist, DEC);
Serial.println(" cm");
}
//*/
// for the motors
void backWard(){
digitalWrite(inA1, HIGH);
digitalWrite(inA2, LOW);
digitalWrite(inB1, HIGH);
digitalWrite(inB2, LOW);
analogWrite(speedPinA, 255);
analogWrite(speedPinB, 255);
}
void forward(){
digitalWrite(inA1, LOW);
digitalWrite(inA2, HIGH);
digitalWrite(inB1, LOW);
digitalWrite(inB2, HIGH);
analogWrite(speedPinA, 255);
analogWrite(speedPinB, 255);
// if the rover is driving down a slope
}
void sTop(){
digitalWrite(inA1, LOW);
digitalWrite(inA2, LOW);
digitalWrite(inB1, LOW);
digitalWrite(inB2, LOW);
analogWrite(speedPinB, 0);
analogWrite(speedPinA, 0);
}
void turnLeft(){
digitalWrite(inA1, HIGH);
digitalWrite(inA2, LOW);
digitalWrite(inB1, LOW);
digitalWrite(inB2, HIGH);
analogWrite(speedPinA, 255);
analogWrite(speedPinB, 255);
}
void turnRight(){
digitalWrite(inA1, LOW);
digitalWrite(inA2, HIGH);
digitalWrite(inB1, HIGH);
digitalWrite(inB2, LOW);
analogWrite(speedPinA, 255);
analogWrite(speedPinB, 255);
}
void compareDistance()
{
if (leftDistance > rightDistance) //if left is less obstructed
{
turnLeft();//turn left
delay(2000);
}
else if (rightDistance > leftDistance) //if right is less obstructed
{
turnRight();//turn right
delay(1000);
}
else //if they are equally obstructed
{
backWard();
delay(400);
turnRight(); //turn 180 degrees
delay(2000);
}
}