Hey guys,
I'm trying to combine two scripts I've found. One to get my remote control model to work via a controller and the other to enable 6 ultrasound modules I have. When I omit the code highlighted in "THIS SECTION" (for the ultrasound modules) the controller and model works fine but when I put it in, the controller becomes unresponsive and the wheels spin uncontrollably.
I'm assuming it's something to do with having what should be two seperate loop sections combined into one?
Any help is much appreciated,
Thanks
#include <PS2X_lib.h> //for v1.6 PS2 Library
//This section defines all the pin data and to be used within the program loop.
//The following four lines set the pin numbers for the PS2 controller.
#define PS2_DAT 11 //Data pin.
#define PS2_CMD 10 //Command pin.
#define PS2_SEL 9 //Attention pin.
#define PS2_CLK 8 //Clock pin.
//The following six lines set the pin numbers for the motor module.
#define IN1 33 //Positive polarity for the right sided wheels.
#define IN2 35 //Negative polarity for the right sided wheels.
#define IN3 37 //Positive polarity for the left sided wheels.
#define IN4 39 //Negative polarity for the left sided wheels.
#define ENA 31 //Jumper pin start.
#define ENB 41 //Jumper pin end.
//The analogue values read from the PS2 contoller.
int LX = 0;
int LY = 0;
int RX = 0;
int RY = 0;
//The following twelve lines set pin numbers for the ultrasonic sensors.
int trigPin1=7; //Front transmitter (Object ahead detection).
int echoPin1=6; //Front receiver.
int trigPin2=5; //Right transmitter (Lane keeping).
int echoPin2=4; //Right receiver.
int trigPin3=3; //Rear right transmitter (Right blindspot detection).
int echoPin3=2; //Rear right receiver.
int trigPin4=14; //Rear centre transmitter (Parking sensor).
int echoPin4=15; //Rear centre receiver.
int trigPin5=16; //Rear left transmitter (Left blindspot detection).
int echoPin5=17; //Rear left receiver.
int trigPin6=19; //Left transmitter (Lane keeping).
int echoPin6=18; //Left receiver.
// This section sets the modes of PS2 controller.
//#define pressures true
#define pressures false
#define rumble true
//#define rumble false
PS2X ps2x; //Creates PS2 controller class.
int error = 0;
byte type = 0;
byte vibrate = 0;
void setup() {
Serial.begin (9600); //Opens the sensor serial ports and sets data rate to 9600bps
pinMode(trigPin1, OUTPUT);
pinMode(echoPin1, INPUT);
pinMode(trigPin2, OUTPUT);
pinMode(echoPin2, INPUT);
pinMode(trigPin3, OUTPUT);
pinMode(echoPin3, INPUT);
pinMode(trigPin4, OUTPUT);
pinMode(echoPin4, INPUT);
pinMode(trigPin5, OUTPUT);
pinMode(echoPin5, INPUT);
pinMode(trigPin6, OUTPUT);
pinMode(echoPin6, INPUT);
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
pinMode(ENA, OUTPUT);
pinMode(ENB, OUTPUT);
digitalWrite(ENA, LOW);
digitalWrite(ENB, LOW);
delay(300); //Added delay to give the wireless PS2 module time to start up before configuring it.
//Setup pins and settings: GamePad(clock, command, attention, data, pressures,rumble?)
error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
if (error == 0) {
Serial.print("Found Controller, configured successful ");
}
else if (error == 1)
Serial.print("No controller found, check wiring");
else if (error == 2)
Serial.print("Controller found but not accepting commands");
else if (error == 3)
Serial.print("Controller refusing to enter Pressures mode, may not support it.");
}
void loop() {
------------------------------SECTION BELOW------------------------------------
long duration1, distance1;
digitalWrite(trigPin1, LOW); // Added this line
delayMicroseconds(2); // Added this line
digitalWrite(trigPin1, HIGH);
delayMicroseconds(10); // Added this line
digitalWrite(trigPin1, LOW);
duration1 = pulseIn(echoPin1, HIGH);
distance1 = (duration1/2) / 29.1;
if (distance1 >= 500 || distance1 <= 0){
Serial.println("Out of range");
}
else {
Serial.print ( "Sensor1 ");
Serial.print ( distance1);
Serial.println("cm");
}
long duration2, distance2;
digitalWrite(trigPin2, LOW); // Added this line
delayMicroseconds(2); // Added this line
digitalWrite(trigPin2, HIGH);
delayMicroseconds(10); // Added this line
digitalWrite(trigPin2, LOW);
duration2 = pulseIn(echoPin2, HIGH);
distance2 = (duration2/2) / 29.1;
if (distance2 >= 500 || distance2 <= 0){
Serial.println("Out of range");
}
else {
Serial.print("Sensor2 ");
Serial.print(distance2);
Serial.println("cm");
}
long duration3, distance3;
digitalWrite(trigPin3, LOW); // Added this line
delayMicroseconds(2); // Added this line
digitalWrite(trigPin3, HIGH);
delayMicroseconds(10); // Added this line
digitalWrite(trigPin3, LOW);
duration3 = pulseIn(echoPin3, HIGH);
distance3 = (duration3/2) / 29.1;
if (distance3 >= 500 || distance3 <= 0){
Serial.println("Out of range");
}
else {
Serial.print("Sensor3 ");
Serial.print(distance3);
Serial.println("cm");
}
long duration4, distance4;
digitalWrite(trigPin4, LOW); // Added this line
delayMicroseconds(2); // Added this line
digitalWrite(trigPin4, HIGH);
delayMicroseconds(10); // Added this line
digitalWrite(trigPin4, LOW);
duration4 = pulseIn(echoPin4, HIGH);
distance4 = (duration4/2) / 29.1;
if (distance4 >= 500 || distance4 <= 0){
Serial.println("Out of range");
}
else {
Serial.print("Sensor4 ");
Serial.print(distance4);
Serial.println("cm");
}
long duration5, distance5;
digitalWrite(trigPin5, LOW);
delayMicroseconds(2);
digitalWrite(trigPin5, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin5, LOW);
duration5 = pulseIn(echoPin5, HIGH);
distance5 = (duration5/2) / 29.1;
if (distance5 >= 500 || distance5 <= 0){
Serial.println("Out of range");
}
else {
Serial.print("Sensor5 ");
Serial.print(distance5);
Serial.println("cm");
}
long duration6, distance6;
digitalWrite(trigPin6, LOW);
delayMicroseconds(2);
digitalWrite(trigPin6, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin6, LOW);
duration6 = pulseIn(echoPin6, HIGH);
distance6 = (duration6/2) / 29.1;
if (distance6 >= 500 || distance6 <= 0){
Serial.println("Out of range");
}
else {
Serial.print("Sensor6 ");
Serial.print(distance6);
Serial.println("cm");
}
-------------------------------------END---------------------------------
ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed
delay(50);
if (ps2x.Button(PSB_L1))
{
LY = ps2x.Analog(PSS_LY); //receive values from p22 joystick
LX = ps2x.Analog(PSS_LX);
}
if (ps2x.Button(PSB_R1))
{
RY = ps2x.Analog(PSS_RY);
RX = ps2x.Analog(PSS_RX);
}
if (LY > 200 || RY > 200) //check if the joystick pushed up side
{
REV();
}
if (LY < 100 || RY < 100)
{
forward();
}
if (LX < 100 || RX < 100)
{
left();
}
if (LX > 200 || RX > 200)
{
right();
}
if(LX == 128 && LY == 128 && RX == 128 && RY == 128)
{
waithere();
}
LY = LX = 128; //return to default vlaues
RY = RX = 128; //return to default values
}
void forward()
{
digitalWrite(ENA, HIGH);
digitalWrite(ENB, HIGH);
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
}
void REV()
{
digitalWrite(ENA, HIGH);
digitalWrite(ENB, HIGH);
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
}
void left()
{
digitalWrite(ENA, LOW);
digitalWrite(ENB, HIGH);
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
}
void right()
{
digitalWrite(ENA, HIGH);
digitalWrite(ENB, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, LOW);
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
}
void waithere()
{
digitalWrite(ENA, LOW);
digitalWrite(ENB, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, LOW);
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
}
