Yeah I'm sorry if it was a bit unclear, I didn't want to post the whole code since I thought it might make it cluttered but I'll add that now. And I am severely lacking in the basics of the coding language since I'm doing it for a university project in a short amount of time and we haven't actually been taught any of it, so I'm just trying to learn what I can in the time period I have.
As for the project itself:
We have to design a vehicle that is "one-button" start to navigate the track from the start to end positions, climbing the two steps. I've attached a picture of the track and the vehicle (the arrows indicate sensors). Basically the track is all flat with no walls except the low wall that I've labelled and the 2 steps. Basically the commands we want to use are:
- drive until the back sensors read a certain distance (eg. 5cm) and then stop {this is to detect the first step}
- realign the vehicle using the distance values of the 2 back sensors.
- Continue driving until the back sensors read the distance again (eg. 5cm), {this will occur after the vehicle has climbed the second step.}
- Rotate 90 degrees counter-clockwise until the value of the 2 left sensors are equal
- drive a certain distance to the finish line then stop.
#define ds_pinb1 0 //s // Pin to which the distance sensor is connected
#define ds_pinb2 1 //s //b1 refers to 1st sensor on back, b2 is second sensor on back, s1 refers to 1st pin on left side etc. Pins are counted in a clockwise direction.
#define ds_pins1 2 //s
#define ds_pins2 3 //s
#define buttonPin 4 //b
int motor_left[] = {2, 3}; //m ////////CHECK THESE
int motor_right[] = {7, 8}; //m ////////NUMBERS//////these are arrays, so motor_right[0]=7 and motor_right[1]=8 etc.
int ds_valueb1; //s // Integer to hold the sensor’s reading
int ds_valueb2; //s
int ds_values1; //s
int ds_values2; //s
float ds_scaledValueb1; //s // Distance sensor’s scaled value
float ds_scaledValueb2; //s
float ds_scaledValues1; //s
float ds_scaledValues2; //s
void setup()
{
Serial.begin (9600); // Initiates serial communication at 9600 baud
int i; //m
for(i = 0; i < 2; i++){ //m
pinMode(motor_left[i], OUTPUT); //m
pinMode(motor_right[i], OUTPUT); //m
}
}
void loop (){
sensor();
sum_value();
int sum_sensor;
drive_forward();
if (ds_scaledValueb1 ==50 || ds_scaledValueb2 == 50) //50 is in mm, may have to change calibration to get this value
{
motor_stop();
if (sum_sensor<100){ //this vale of 100 has to be determined, it will just be a value greater than the total distance the sensor has read as the vehicle stops this first step (just to differentiate between first and second step)
while (ds_scaledValueb1 != ds_scaledValueb2){
if (ds_scaledValueb1 > ds_scaledValueb2)
turn_right();
else if (ds_scaledValueb1 < ds_scaledValueb2)
turn_left();}
drive_forward();
delay(200);}
else if (sum_sensor>100){
while (ds_scaledValues1 != ds_scaledValues2){
if (ds_scaledValues2 > ds_scaledValues1){
turn_left();}
else if (ds_scaledValues1 > ds_scaledValues2) //this part is just in case the motor turns too far
turn_right();}
drive_forward();}
delay(10000); //time delay has to be determined, based on distance the vehicle needs to travel to finish in end zone.
motor_stop();}}
// --------------------------------------------------------------------------- Drive
void sum_value(){
int sum_sensor;
sum_sensor=sum_sensor+(ds_scaledValueb1+ds_scaledValueb2)/2;
void sensor(){
ds_valueb1 = analogRead (ds_pinb1); //s // Reads the distance sensor
ds_valueb2 = analogRead (ds_pinb2); //s
ds_values1 = analogRead (ds_pins1); //s
ds_values2 = analogRead (ds_pins2); //s
ds_scaledValueb1 = ((float)ds_valueb1 / 615) * 1024; //s // Calculates the scaled value
ds_scaledValueb2 = ((float)ds_valueb2 / 615) * 1024; //s
ds_scaledValues1 = ((float)ds_values1 / 615) * 1024; //s
ds_scaledValues2 = ((float)ds_values2 / 615) * 1024; //s
}
void motor_stop(){
digitalWrite(motor_left[0], LOW);
digitalWrite(motor_left[1], LOW);
digitalWrite(motor_right[0], LOW);
digitalWrite(motor_right[1], LOW);
delay(25);
}
void drive_forward(){
digitalWrite(motor_left[0], HIGH);
digitalWrite(motor_left[1], LOW);
digitalWrite(motor_right[0], HIGH);
digitalWrite(motor_right[1], LOW);
}
void drive_backward(){
digitalWrite(motor_left[0], LOW);
digitalWrite(motor_left[1], HIGH);
digitalWrite(motor_right[0], LOW);
digitalWrite(motor_right[1], HIGH);
}
void turn_left(){
digitalWrite(motor_left[0], LOW);
digitalWrite(motor_left[1], HIGH);
digitalWrite(motor_right[0], HIGH);
digitalWrite(motor_right[1], LOW);
}
void turn_right(){
digitalWrite(motor_left[0], HIGH);
digitalWrite(motor_left[1], LOW);
digitalWrite(motor_right[0], LOW);
digitalWrite(motor_right[1], HIGH);
}
Sorry for the lack of detail at the start of the post, hopefully this clears things up a bit
