Skip to content

Instantly share code, notes, and snippets.

@cdwijayarathna

cdwijayarathna/IESL RoboGames Code

Created November 24, 2013 10:35
Show Gist options
  • Save cdwijayarathna/7625724 to your computer and use it in GitHub Desktop.
Save cdwijayarathna/7625724 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
IESL RoboGames Code
#include <QTRSensors.h>
#include <QTRSensors.h>
#include <LiquidCrystal.h>
#include <Encoder.h>
#include <Servo.h>
#include <Wire.h>
#include <LSM303.h>
Servo myservo; // create servo object to control a servo
LSM303 compass;
Encoder knobLeft(2, 2);
Encoder knobRight(3, 3);
long newLeft, newRight=0;
int motorSpeed,lastError=0;
int error=0;
unsigned int position=0;
unsigned int positionBar=0;
int Minval,Maxval,Midval=0;
int pwm,Lpwm,Rpwm=0;
int i = 0;
boolean lastSensor=0;
LiquidCrystal lcd(41, 23, 25, 27, 29, 31);
#define NUM_SENSORS 8 // number of sensors used
#define TIMEOUT 2500 // waits for 2500 microseconds for sensor outputs to go low
#define EMITTER_PIN QTR_NO_EMITTER_PIN // emitter is controlled by digital pin 2
#define NUM_SENSORS_BAR 3 // number of sensors used
// sensors 0 through 7 are connected to digital pins 3 through 10, respectively
QTRSensorsRC qtrrc((unsigned char[]) {
36, 38, 40, 42, 44, 46, 48, 50}
,NUM_SENSORS, TIMEOUT, EMITTER_PIN);
QTRSensorsRC qtrrcBar((unsigned char[]) {
42, 44, 46,}
, NUM_SENSORS_BAR, TIMEOUT, EMITTER_PIN);
QTRSensorsRC qtrrcBar2((unsigned char[]) {
36 ,38 ,40}
, NUM_SENSORS_BAR, TIMEOUT, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];
unsigned int sensorValuesBar[NUM_SENSORS_BAR];
boolean sensorValuesBool[NUM_SENSORS];
int irThreshold = 100;
int sensorCount;
boolean junctionSize[16]; // 0 fro 3 way, 1 for 4 way
int treasureCount[16];
int reached[16];
int nextJunction;
int previousJunction;
int neighbours[16][4];
boolean covered[16][4];
int compassVal[16][4];
boolean barCode[9];
int state;
int juncCount;
boolean onPath;
unsigned int totEncorder;
int bestChoice = 0;
int previousState;
int currentDir = 0;
int turnDirection;
boolean reachedBefore;
int maxJunction;
void setup()
{
delay(1000);
nextJunction = 0;
pinMode(43, INPUT);
pinMode(22, INPUT);
pinMode(33,OUTPUT);
pinMode(35,OUTPUT);
pinMode(37,OUTPUT);
pinMode(39,OUTPUT);
for(int i=0;i<16;i++){
junctionSize[i] = 0;
reached[i]=0;
treasureCount[i]=-1;
for(int j=0;j<4;j++){
neighbours[i][j]=0;
covered[i][j]=0;
}
}
lcd.begin(16, 2);
lcd.print("Sens Calibtn");
knobLeft.write(0);
knobRight.write(0);
while( knobLeft.read() + knobRight.read() <40000 )
{
while( knobLeft.read() + knobRight.read() <10000 ){
clockwise(60);
qtrrc.calibrate(); // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call)
qtrrcBar.calibrate();
}
Active_brk(255);
while ( knobLeft.read() + knobRight.read() <30000 ){
antiClockwise(60);
qtrrc.calibrate(); // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call)
qtrrcBar.calibrate();
}
Active_brk(255);
while( knobLeft.read() + knobRight.read() <40000 ){
clockwise(60);
qtrrc.calibrate(); // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call)
qtrrcBar.calibrate();
}
}
knobLeft.write(0);
knobRight.write(0);
Active_brk(255);
lcd.clear();
lcd.print("Calibrated");
delay(500);
Serial.begin(9600);
state=7;
reachedBefore = 0;
pinMode(8, INPUT);
pinMode(9, INPUT);
pinMode(10, INPUT);
pinMode(11, INPUT);
pinMode(12, INPUT);
pinMode(13, INPUT);
pinMode(14, INPUT);
pinMode(15, INPUT);
previousJunction = 0;
nextJunction = 0;
for (int i=0;i<16;i++){
treasureCount[i] = -1;
}
//treasureCount[3] = 0;
//treasureCount[7] = ;
digitalWrite(22, HIGH);
Wire.begin();
compass.init();
compass.enableDefault();
// Calibration values. Use the Calibrate example program to get the values for
// your compass.
// compass.m_min.x = +94;
// compass.m_min.y = -481;
// compass.m_min.z = -185;
// compass.m_max.x = +812;
// compass.m_max.y = +360;
// compass.m_max.z = -71;
compass.m_min.x = 996; compass.m_min.y = -70; compass.m_min.z = 62;
compass.m_max.x = 1008; compass.m_max.y = -58; compass.m_max.z = 71;
myservo.attach(10); // attaches the servo on pin 9 to the servo object
myservo.write(4); // sets the servo position according to the scaled value
delay(500); // waits for the servo to get there
myservo.detach();
maxJunction = 5;
}
int compassReading(){
compass.read();
int heading = compass.heading((LSM303::vector){
0,-1,0 }
);
return heading;
}
int direction;
int getReverseDirection(){
direction = compassReading();
int reverseDirection = direction + 180;
if(reverseDirection >=360){
reverseDirection -= 360;
}
return reverseDirection;
}
void setSensorValues(){
sensorCount=0;
Serial.println(position);
for(int i=0; i<NUM_SENSORS;i++){
// Serial.print(sensorValues[i]);
// Serial.print("\t");
if(sensorValues[i] < 500){
sensorValuesBool[i]=1;
sensorCount++;
}
else{
sensorValuesBool[i] = 0;
}
}
//Serial.println();
}
/////////////////////////////////////////////////////////////
void left_mtr_fw(int pwm){
digitalWrite(33, HIGH);
digitalWrite(35, LOW);
analogWrite(8,pwm);
}
void right_mtr_fw(int pwm){
digitalWrite(39, HIGH);
digitalWrite(37, LOW);
analogWrite(9,pwm);
}
void left_mtr_rv(int pwm){
digitalWrite(33, LOW);
digitalWrite(35, HIGH);
analogWrite(8,pwm);
}
void right_mtr_rv(int pwm){
digitalWrite(39, LOW);
digitalWrite(37, HIGH);
analogWrite(9,pwm);
}
void Active_brk(int pwm){
for(int i=0;i<4;i++){
analogWrite(8,pwm);
analogWrite(9,pwm);
digitalWrite(33, HIGH);
digitalWrite(35, HIGH);
digitalWrite(37, HIGH);
digitalWrite(39, HIGH);
delay(10);
analogWrite(8,0);
analogWrite(9,0);
delay(10);
}
}
void Forwd(int pwm_left,int pwm_right){
left_mtr_fw(pwm_left);
right_mtr_fw(pwm_right);
}
void Back(int pwm_left,int pwm_right){
left_mtr_rv(pwm_left);
right_mtr_rv(pwm_right);
}
void clockwise(int pwd){
left_mtr_rv(pwd);
right_mtr_fw(pwd);
}
void antiClockwise(int pwd){
left_mtr_fw(pwd);
right_mtr_rv(pwd);
}
void stopAll(){
digitalWrite(33, HIGH);
digitalWrite(35, HIGH);
digitalWrite(37, HIGH);
digitalWrite(39, HIGH);
}
void barLineFollow(float KP,float KD,float KI,int Mid,int Max,int Min){
positionBar = qtrrcBar.readLine(sensorValuesBar,1,1); //513 at middle leftmost 4000 rightmost 0
error = positionBar - 845;
//Serial.println(positionBar);
motorSpeed = KP * error + KD * (error - lastError);
lastError = error;
Lpwm=Mid+motorSpeed;
Rpwm=Mid-motorSpeed;
if(Lpwm>Max)
Lpwm=Max;
else if(Lpwm<Min)
Lpwm=Min;
if(Rpwm>Max)
Rpwm=Max;
else if(Rpwm<Min)
Rpwm=Min;
Forwd(Rpwm,Lpwm);
// lcd.clear();
// lcd.setCursor(0,0);
// lcd.print(Lpwm);
// lcd.setCursor(0,1);
// lcd.print(Rpwm);
}
void barLineFollow2(float KP,float KD,float KI,int Mid,int Max,int Min){
positionBar = qtrrcBar.readLine(sensorValuesBar,1,1); //513 at middle leftmost 4000 rightmost 0
error = positionBar - 845;
//Serial.println(positionBar);
motorSpeed = KP * error + KD * (error - lastError);
lastError = error;
Lpwm=Mid+motorSpeed;
Rpwm=Mid-motorSpeed;
if(Lpwm>Max)
Lpwm=Max;
else if(Lpwm<Min)
Lpwm=Min;
if(Rpwm>Max)
Rpwm=Max;
else if(Rpwm<Min)
Rpwm=Min;
Forwd(Rpwm,Lpwm);
// lcd.clear();
// lcd.setCursor(0,0);
// lcd.print(Lpwm);
// lcd.setCursor(0,1);
// lcd.print(Rpwm);
}
void linefollowing(float KP,float KD,float KI,int Mid,int Max,int Min){
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
if(sensorValuesBool[0] != sensorValuesBool[7] ){
if(sensorValuesBool[0] > sensorValuesBool[7]){
lastSensor=1;
}
else
lastSensor=0;
}
//Serial.print(lastSensor);
//Serial.print(" ");
//Serial.println(position);
if(position == 0 || position == 7000){
Serial.println("junc");
knobLeft.write(0);
knobRight.write(0);
lcd.clear();
lcd.print("Junction");
while((knobLeft.read()+knobRight.read())<11000){
lcd.clear();
lcd.setCursor(0,0);
lcd.print(knobLeft.read());
lcd.setCursor(0,1);
lcd.print(knobRight.read());
Forwd(100,100);
// Serial.println("went forwd");
}
Active_brk(255);
if(lastSensor == 0){
lcd.clear();
// lcd.print("Turn Left");
while (sensorValuesBool[3]==0 || sensorValuesBool[4]==0){
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
clockwise(100);
}
//Serial.println("turned left");
Active_brk(255);
///////////////////////////////////////////////////////////////////////////////////////
knobLeft.write(0);
knobRight.write(0);
while ((knobLeft.read()+knobRight.read())<6000){
clockwise(60);
}
knobLeft.write(0);
knobRight.write(0);
//Serial.println("turned left encoder 6000");
Active_brk(255);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
while (sensorValuesBool[4]==0){
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
antiClockwise(60);
}
//Serial.println("turned left sensor[4]");
Active_brk(255);
}
if(lastSensor == 1){
lcd.clear();
// lcd.print("Turn right");
while (sensorValuesBool[3]==0 || sensorValuesBool[4]==0){
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
antiClockwise(100);
}
// Serial.println("turned right");
Active_brk(255);
}
// Serial.println("came out");
}
else{
error = position - 3500;
motorSpeed = KP * error + KD * (error - lastError);
lastError = error;
Lpwm=Mid+motorSpeed;
Rpwm=Mid-motorSpeed;
if(Lpwm>Max)
Lpwm=Max;
else if(Lpwm<Min)
Lpwm=Min;
if(Rpwm>Max)
Rpwm=Max;
else if(Rpwm<Min)
Rpwm=Min;
Forwd(Rpwm,Lpwm);
lcd.clear();
lcd.setCursor(0,0);
Serial.print(analogRead(9));
Serial.print(" ");
Serial.print(Lpwm);
Serial.print(" ");
Serial.print(Rpwm);
Serial.println(" ");
error = position - 3500;
lcd.setCursor(0,1);
//lcd.print(Rpwm);
}
}
////////////////////////////*************///////////////////////////**************
void linefollowing2(float KP,float KD,float KI,int Mid,int Max,int Min){
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
if(sensorValuesBool[0] != sensorValuesBool[7] ){
if(sensorValuesBool[0] > sensorValuesBool[7]){
lastSensor=1;
}
else
lastSensor=0;
}
//Serial.print(lastSensor);
//Serial.print(" ");
//Serial.println(position);
if(position == 0 || position == 7000){
Serial.println("junc");
knobLeft.write(0);
knobRight.write(0);
lcd.clear();
lcd.print("Junction");
while((knobLeft.read()+knobRight.read())<11000){
// lcd.clear();
// lcd.setCursor(0,0);
// lcd.print(knobLeft.read());
// lcd.setCursor(0,1);
// lcd.print(knobRight.read());
Forwd(100,100);
// Serial.println("went forwd");
}
Active_brk(255);
if(lastSensor == 1){
lcd.clear();
// lcd.print("Turn Left");
while (sensorValuesBool[3]==0 || sensorValuesBool[4]==0){
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
antiClockwise(100);
}
//Serial.println("turned left");
Active_brk(255);
///////////////////////////////////////////////////////////////////////////////////////
knobLeft.write(0);
knobRight.write(0);
while ((knobLeft.read()+knobRight.read())<6000){
antiClockwise(60);
}
knobLeft.write(0);
knobRight.write(0);
//Serial.println("turned left encoder 6000");
Active_brk(255);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
while (sensorValuesBool[4]==0){
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
clockwise(60);
}
//Serial.println("turned left sensor[4]");
Active_brk(255);
}
if(lastSensor == 0){
lcd.clear();
// lcd.print("Turn right");
while (sensorValuesBool[1]==0){
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
clockwise(100);
}
knobLeft.write(0);
knobRight.write(0);
while((knobLeft.read()+knobRight.read())<4000){
linefollowing3(0.031,0,0,160,255,10);
}
knobLeft.write(0);
knobRight.write(0);
// Serial.println("turned right");
Active_brk(255);
}
// Serial.println("came out");
}
else{
error = position - 3500;
motorSpeed = KP * error + KD * (error - lastError);
lastError = error;
Lpwm=Mid+motorSpeed;
Rpwm=Mid-motorSpeed;
if(Lpwm>Max)
Lpwm=Max;
else if(Lpwm<Min)
Lpwm=Min;
if(Rpwm>Max)
Rpwm=Max;
else if(Rpwm<Min)
Rpwm=Min;
Forwd(Rpwm,Lpwm);
lcd.clear();
lcd.setCursor(0,0);
// Serial.print(Lpwm);
// Serial.print(" ");
// Serial.print(Rpwm);
// Serial.println(" ");
error = position - 3500;
lcd.setCursor(0,1);
//lcd.print(Rpwm);
}
}
//////////////////***********************//////////////////////////////////////////////
//line follow 3
void linefollowing3(float KP,float KD,float KI,int Mid,int Max,int Min){
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
if(sensorValuesBool[0] != sensorValuesBool[7] ){
if(sensorValuesBool[0] > sensorValuesBool[7]){
lastSensor=1;
}
else
lastSensor=0;
}
//Serial.print(lastSensor);
//Serial.print(" ");
//Serial.println(position);
if(position == 0 || position == 7000){
Serial.println("junc");
knobLeft.write(0);
knobRight.write(0);
lcd.clear();
lcd.print("Junction");
while((knobLeft.read()+knobRight.read())<11000){
// lcd.clear();
// lcd.setCursor(0,0);
// lcd.print(knobLeft.read());
// lcd.setCursor(0,1);
// lcd.print(knobRight.read());
Forwd(100,100);
// Serial.println("went forwd");
}
Active_brk(255);
if(lastSensor == 1){
lcd.clear();
// lcd.print("Turn Left");
while (sensorValuesBool[3]==0 || sensorValuesBool[4]==0){
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
antiClockwise(100);
}
//Serial.println("turned left");
Active_brk(255);
///////////////////////////////////////////////////////////////////////////////////////
knobLeft.write(0);
knobRight.write(0);
while ((knobLeft.read()+knobRight.read())<6000){
antiClockwise(60);
}
knobLeft.write(0);
knobRight.write(0);
//Serial.println("turned left encoder 6000");
Active_brk(255);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
while (sensorValuesBool[4]==0){
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
clockwise(60);
}
//Serial.println("turned left sensor[4]");
Active_brk(255);
}
if(lastSensor == 0){
lcd.clear();
// lcd.print("Turn right");
while (sensorValuesBool[1]==0){
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
clockwise(100);
}
// Serial.println("turned right");
Active_brk(255);
}
// Serial.println("came out");
}
else{
error = position - 3500;
motorSpeed = KP * error + KD * (error - lastError);
lastError = error;
Lpwm=Mid+motorSpeed;
Rpwm=Mid-motorSpeed;
if(Lpwm>Max)
Lpwm=Max;
else if(Lpwm<Min)
Lpwm=Min;
if(Rpwm>Max)
Rpwm=Max;
else if(Rpwm<Min)
Rpwm=Min;
Forwd(Rpwm,Lpwm);
lcd.clear();
lcd.setCursor(0,0);
// Serial.print(Lpwm);
// Serial.print(" ");
// Serial.print(Rpwm);
// Serial.println(" ");
error = position - 3500;
lcd.setCursor(0,1);
//lcd.print(Rpwm);
}
}
/******************************////////////////
/*/////////////////////*/////////////////////////*//////////////////
int barNumber = 0;
int encorderValue;
int readBarCode(){
//Forwd(20,20);
barLineFollow(0.125,0,0,35,90,10) ;
//Serial.println(analogRead(14));
// Serial.print(analogRead(8));
// Serial.print("\t");
// Serial.print(analogRead(9));
// Serial.print("\t");
// Serial.print(analogRead(10));
// Serial.print("\t");
// Serial.print(analogRead(11));
// Serial.print("\t");
// Serial.print(analogRead(12));
// Serial.print("\t");
// Serial.print(analogRead(13));
// Serial.print("\t");
// Serial.print(analogRead(14));
// Serial.print("\t");
// Serial.print(analogRead(15));
// Serial.println("\t");
if(analogRead(14) > irThreshold && barNumber%2 == 0){
Serial.print(knobLeft.read());
Serial.print(" ");
Serial.print(knobRight.read());
Serial.println(" ");
// Active_brk(255);
// delay(100);
encorderValue = (knobLeft.read() + knobRight.read()) / 2;
if(barNumber > 0 && barNumber < 10){
Serial.print(barNumber);
Serial.print(" ");
if(encorderValue >=750){
barCode[barNumber - 1] = 1;
Serial.println("1");
}
else {
barCode[barNumber -1] = 0;
Serial.println("0");
}
}
barNumber ++;
totEncorder = totEncorder + knobLeft.read()+knobRight.read();
knobLeft.write(0);
knobRight.write(0);
}
else if(analogRead(14) < irThreshold && barNumber%2 != 0){
Serial.print(knobLeft.read());
Serial.print(" ");
Serial.print(knobRight.read());
Serial.println();
// Active_brk(255);
// delay(100);
encorderValue = (knobLeft.read() + knobRight.read()) / 2;
if(barNumber > 0 && barNumber < 10){
Serial.print(barNumber);
Serial.print(" ");
if(encorderValue >=350){
barCode[barNumber - 1] = 1;
Serial.println("1");
}
else {
barCode[barNumber -1] = 0;
Serial.println("0");
}
}
barNumber ++;
totEncorder = totEncorder + knobLeft.read()+knobRight.read();
knobLeft.write(0);
knobRight.write(0);
}
if(barNumber == 10 ){
if( (barCode[0] + barCode[1] + barCode[2] + barCode[3] + barCode[4] + barCode[5] + barCode[6] + barCode[7] + barCode[8]) % 2 == 1){
//update junction treasure details and change state
barNumber = 0;
Serial.println("BarCode Return");
return 1;
}
else{
lcd.clear();
lcd.print("Failure");
barNumber=0;
return 2;
}
//barNumber = 0;
}
return 0;
}
int result;
int right, left;
void loop()
{
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
// for (int i = 0; i < NUM_SENSORS; i++)
// {
// Serial.print(sensorValues[i]);
// Serial.print(' ');
// }
// Serial.println();
// if(state == 0){
// lcd.print("State 0");
// linefollowing(0.0516,0,0,100,255,30);
// if(sensorCount>3){
// state = 1;
// }
// }
//
// //On startingLine
// else if(state == 1){
// lcd.print("State 1");
// Forwd(30,30);
// if(analogRead(14)< irThreshold || analogRead(9)< irThreshold){
// while(analogRead(14)< irThreshold || analogRead(9)< irThreshold){
// Forwd(30,30);
// }
// state = 2;
// }
// }
//1st path before barcode
if (state == 1){
lcd.print("State 1");
//linefollowing(0.054271,0,0,180,255,30);
linefollowing2(0.031,0,0,160,255,10);
if(analogRead(14) < 500 && Lpwm - Rpwm > -75 && Lpwm - Rpwm < 75 && analogRead(15) > 500){
knobLeft.write(0);
knobRight.write(0);
Active_brk(255);
delay(100);
//Serial.println("State changed to 1");
barNumber = 0;
while(knobLeft.read()+knobRight.read()<10000){
Back(30,30);
}
Active_brk(255);
delay(100);
knobLeft.write(0);
knobRight.write(0);
state = 2;
}
}
else if (state == 2){
lcd.print("State 2");
//linefollowing(0.054271,0,0,180,255,30);
barLineFollow(0.05478,0,0,40,70,30) ;
if(knobLeft.read()+knobRight.read()>50000){
state = 1 ;
}
if(analogRead(14)< irThreshold){
knobLeft.write(0);
knobRight.write(0);
//Serial.println("State changed to 1");
barNumber = 0;
state = 3;
}
}
// 1st path bar code
else if(state == 3){
lcd.print("State 3");
if(knobLeft.read()+knobRight.read()>15000){
state = 1 ;
}
if(readBarCode() == 1){
nextJunction = barCode[0] * 8 + barCode[1] * 4 + barCode[2] * 2 + barCode[3] * 1 ;
if(nextJunction > maxJunction){
maxJunction = nextJunction;
}
treasureCount[nextJunction-1] = barCode[4] * 8 + barCode[5] * 4 + barCode[6] * 2 + barCode[7] * 1;
Serial.println();
Serial.print(nextJunction);
Serial.print(" ");
Serial.print(treasureCount[nextJunction-1]);
lcd.clear();
lcd.setCursor(0,0);
lcd.print(barCode[0] * 8 + barCode[1] * 4 + barCode[2] * 2 + barCode[3] * 1);
lcd.setCursor(4,0);
lcd.print(barCode[4] * 8 + barCode[5] * 4 + barCode[6] * 2 + barCode[7] * 1);
state =4;
myservo.attach(10); // attaches the servo on pin 9 to the servo object
myservo.write(4); // sets the servo position according to the scaled value
delay(500); // waits for the servo to get there
myservo.detach();
}
}
//1st path from start, bar code passed
else if (state == 4){
lcd.print("State 4");
linefollowing(0.031,0,0,160,255,10);
Serial.print(digitalRead(22));
if(digitalRead(22)==0){
state=10;
previousState = 4;
linefollowing(0.04848,0,0,90,255,10);
lcd.clear();
lcd.print("Coin found");
}
if(sensorCount>4){
//CHANGE - should go to 5 only if junction is not reached before, otherwise calculate next junction and go to it
lcd.clear();
lcd.print("Suspect");
right = 0;
left = 0;
knobLeft.write(0);
knobRight.write(0);
Active_brk(255);
delay(100);
//lcd.print("Junc state8");
while(knobLeft.read() + knobRight.read() < 3000){
//lcd.print("Back");
//lcd.clear();
Back(40,40);
}
Active_brk(255);
delay(100);
right =0;
left=0;
knobLeft.write(0);
knobRight.write(0);
while(knobLeft.read() + knobRight.read() < 10000){
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
lcd.print("Forward");
lcd.clear();
if(sensorValuesBool[6] ==1){
right ++;
}
if(sensorValuesBool[1] ==1){
left ++;
}
Forwd(50,50);
}
lcd.clear();
lcd.setCursor(0,0);
lcd.print(right);
lcd.setCursor(5,0);
lcd.print(left);
lcd.setCursor(0,1);
lcd.print(sensorValuesBool[3]);
lcd.setCursor(5,1);
lcd.print(sensorValuesBool[4]);
Active_brk(255);
delay(100);
if(right>5 && left > 5 && (sensorValuesBool[3] ==1 || sensorValuesBool[4] ==1)){
lcd.clear();
lcd.print(4);
junctionSize[nextJunction-1] = 1;
}
else if((right>5 && left < 5 && !(sensorValuesBool[3] ==1 || sensorValuesBool[4] ==1)) || (right<5 && left > 5 && !(sensorValuesBool[3] ==1 || sensorValuesBool[4] ==1))){
knobLeft.write(0);
knobRight.write(0);
while(knobLeft.read() + knobRight.read() < 17000){
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
linefollowing(0.02715,0,0,160,255,30);
}
return;
}
else{
lcd.clear();
lcd.print(3);
junctionSize[nextJunction-1] = 0;
}
//lcd.print(right);
lcd.setCursor(0,1);
lcd.print(getReverseDirection());
compassVal[nextJunction-1][0] = getReverseDirection();
covered[nextJunction-1][0] = 1; //*******************************************
while(analogRead(9)>irThreshold && analogRead(14)>irThreshold){
Forwd(80,80);//++++++++++++++++++++++++++++++++++++++++++++++++++++++++
}
knobLeft.write(0);
knobRight.write(0);
while((knobLeft.read()+knobRight.read())<6500){
Forwd(80,80);
}
Active_brk(255);
delay(100);
knobLeft.write(0);
knobRight.write(0);
while((knobLeft.read()+knobRight.read())<25000){
lcd.clear();
lcd.setCursor(0,0);
lcd.print(knobLeft.read());
lcd.setCursor(6,0);
lcd.print(knobRight.read());
clockwise(100);
}
Active_brk(255);
delay(100);
state = 5;
myservo.attach(10); // attaches the servo on pin 9 to the servo object
myservo.write(60); // sets the servo position according to the scaled value
delay(500); // waits for the servo to get there
myservo.detach();
juncCount=1;
onPath=0;
neighbours[nextJunction-1][0]=previousJunction;
return;
}
//linefollowing(0.02715,0,0,140,255,30);
}
//normal line segment, before 2nd barcode
else if(state ==7){
lcd.print("State 7");
linefollowing(0.031,0,0,160,255,10);
Serial.print(analogRead(9));
Serial.print(" ");
Serial.println(analogRead(8));
if(analogRead(9) < 500 && Lpwm - Rpwm > -75 && Lpwm - Rpwm < 75 && analogRead(8) > 500){
Active_brk(255);
Serial.println("State changed");
lcd.clear();
lcd.print("888888888");
delay(100);
state = 8;
}
// else if(analogRead(9) < irThreshold){
// Active_brk(255);
// Serial.println("pwm awl");
// delay(10000);
// }
if(digitalRead(22)==0){
state=10;
previousState = 7;
lcd.clear();
lcd.print("Coin found");
}
}
//near junction after barcode 2
else if (state == 8){
lcd.print("State 8");
linefollowing(0.0484,0,0,90,255,10);
if(sensorCount>4){
//CHANGE - should go to 5 only if junction is not reached before, otherwise calculate next junction and go to it
if(reached[nextJunction-1]==0){
reachedBefore = 0;
right = 0;
left = 0;
knobLeft.write(0);
knobRight.write(0);
Active_brk(255);
delay(100);
lcd.print("Junc state8");
while(knobLeft.read() + knobRight.read() < 3000){
lcd.print("Back");
lcd.clear();
Back(40,40);
}
Active_brk(255);
delay(100);
knobLeft.write(0);
knobRight.write(0);
right =0;
left=0;
while(knobLeft.read() + knobRight.read() < 10000){
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
lcd.print("Forward");
lcd.clear();
if(sensorValuesBool[6] ==1){
right ++;
}
if(sensorValuesBool[1] ==1){
left ++;
}
Forwd(50,50);
}
Serial.print("Right Left ");
Serial.print(sensorValuesBool[3]);
Serial.print("\t");
Serial.println(sensorValuesBool[4]);
Active_brk(255);
delay(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
if(right>5 && left > 5 && (sensorValuesBool[3] ==1 || sensorValuesBool[4] ==1)){
lcd.clear();
Serial.print(4);
junctionSize[nextJunction-1] = 1;
}
else{
lcd.clear();
Serial.print(3);
junctionSize[nextJunction-1] = 0;
}
//lcd.print(right);
lcd.setCursor(0,1);
lcd.print(getReverseDirection());
compassVal[nextJunction-1][0] = getReverseDirection();
covered[nextJunction-1][0] = 1;
while(analogRead(9)>irThreshold && analogRead(14)>irThreshold){
Forwd(80,80);
}
knobLeft.write(0);
knobRight.write(0);
while((knobLeft.read()+knobRight.read())<6500){
Forwd(80,80);
}
Active_brk(255);
delay(100);
knobLeft.write(0);
knobRight.write(0);
while((knobLeft.read()+knobRight.read())<25000){
lcd.clear();
lcd.setCursor(0,0);
lcd.print(knobLeft.read());
lcd.setCursor(6,0);
lcd.print(knobRight.read());
clockwise(100);
}
Active_brk(255);
delay(100);
state = 5;
myservo.attach(10); // attaches the servo on pin 9 to the servo object
myservo.write(60); // sets the servo position according to the scaled value
delay(500); // waits for the servo to get there
myservo.detach();
juncCount=1;
onPath=0;
neighbours[nextJunction-1][0]=previousJunction;
}
else{
///////////JORA KHAN
reachedBefore = 1;
while(analogRead(8) > irThreshold && analogRead(9) > irThreshold && analogRead(15) > irThreshold && analogRead(14) > irThreshold){
Forwd(80,80);
}
Active_brk(255);
delay(100);
knobLeft.write(0);
knobRight.write(0);
while((knobLeft.read()+knobRight.read())<8000){
Forwd(80,80);
}
Active_brk(255);
delay(100);
knobLeft.write(0);
knobRight.write(0);
while((knobLeft.read()+knobRight.read())<36000){
clockwise(100);
}
Active_brk(255);
delay(100);
currentDir = 0;
for(i = 0;i<3;i++){
if(fabs(compassVal[nextJunction-1][currentDir] - compassReading()) > fabs(compassVal[nextJunction-1][i] - compassReading())){
currentDir = i;
}
}
covered[nextJunction-1][currentDir] = 1;
reached[nextJunction-1]++;
Serial.println("Reached");
for(int j = 0; j< 16 ;j++){
Serial.print(reached[j]);
Serial.print(" ");
}
Serial.println();
state = 9;
}
}
}
//On junction. detect barcodes and read
else if(state == 5){
if(onPath == 0 && ((juncCount < 3 && junctionSize[nextJunction -1] == 0) || (juncCount < 4 && junctionSize[nextJunction-1] == 1))){
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
while(sensorValuesBool[4] ==0 ){
antiClockwise(50);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
onPath=1;
Active_brk(255);
delay(100);
totEncorder = 0;
knobLeft.write(0);
knobRight.write(0);
while(knobLeft.read()+knobRight.read()<5000){
barLineFollow(0.05478,0,0,40,70,30) ;
}
while(analogRead(14) > irThreshold){
//deadEnd ------ Tune************************
if( sensorCount ==0 ){
deadEnd2();
return;
}
if(knobLeft.read()+knobRight.read()>35000){
deadEnd2();
return;
}
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
barLineFollow(0.05478,0,0,40,70,30) ;
}
totEncorder = totEncorder + knobLeft.read()+knobRight.read();
Active_brk(255);
delay(100);
knobLeft.write(0);
knobRight.write(0);
while(knobLeft.read()+knobRight.read()<6000){
Back(30,30);
}
Active_brk(255);
delay(100);
totEncorder = totEncorder - knobLeft.read()-knobRight.read();
knobLeft.write(0);
knobRight.write(0);
while(analogRead(14) > irThreshold){
barLineFollow(0.05478,0,0,20,70,30) ;
if( sensorCount ==0 ){
deadEnd2();
return;
}
if(knobLeft.read()+knobRight.read()>35000){
deadEnd2();
return;
}
}
totEncorder = totEncorder + knobLeft.read()+knobRight.read();
knobLeft.write(0);
knobRight.write(0);
// Serial.println("State changed to 1");
barNumber = 0;
// Active_brk(255);
// delay(100);
lcd.clear();
lcd.print("Start reading");
// delay(100);
}
else if(onPath ==1){
lcd.clear();
lcd.print("onPath 1");
//delay(100);
result = readBarCode();
if(result == 1){
//nextJunction = barCode[0] * 8 + barCode[1] * 4 + barCode[2] * 2 + barCode[0] * 1 ;
if(treasureCount[barCode[0] * 8 + barCode[1] * 4 + barCode[2] * 2 + barCode[3] * 1 - 1]<0){
treasureCount[barCode[0] * 8 + barCode[1] * 4 + barCode[2] * 2 + barCode[3] * 1 - 1] = treasureCount[barCode[0] * 8 + barCode[1] * 4 + barCode[2] * 2 + barCode[3] * 1 - 1] + 1 + barCode[4] * 8 + barCode[5] * 4 + barCode[6] * 2 + barCode[7] * 1;
}
if(barCode[0] * 8 + barCode[1] * 4 + barCode[2] * 2 + barCode[3] * 1 > maxJunction){
maxJunction = barCode[0] * 8 + barCode[1] * 4 + barCode[2] * 2 + barCode[3] * 1;
}
neighbours[nextJunction-1][juncCount]=barCode[0] * 8 + barCode[1] * 4 + barCode[2] * 2 + barCode[3] * 1;
lcd.clear();
lcd.setCursor(0,0);
lcd.print(barCode[0] * 8 + barCode[1] * 4 + barCode[2] * 2 + barCode[3] * 1);
lcd.setCursor(4,0);
lcd.print(barCode[4] * 8 + barCode[5] * 4 + barCode[6] * 2 + barCode[7] * 1);
lcd.setCursor(0,1);
lcd.print(compassReading());
//neighbours[nextJunction-1][0]=barCode[0] * 8 + barCode[1] * 4 + barCode[2] * 2 + barCode[0] * 1;
//Serial.println();
//Serial.print(nextJunction);
//Serial.print(" ");
//Serial.print(treasureCount[nextJunction-1]);
//state =4;
Active_brk(255);
delay(100);
totEncorder = totEncorder + knobLeft.read()+knobRight.read();
knobLeft.write(0);
knobRight.write(0);
while(knobLeft.read()+knobRight.read()<totEncorder + 4000){
Back(50,50);
}
compassVal[nextJunction-1][juncCount]=compassReading();
juncCount++;
onPath=0;
Active_brk(255);
delay(100);
if(((juncCount < 3 && junctionSize[nextJunction -1] == 0) || (juncCount < 4 && junctionSize[nextJunction-1] == 1))){
while(sensorValuesBool[3] ==1 || sensorValuesBool[4] ==1){
antiClockwise(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
}
else{
for(int i = 0; i< 16 ; i++){
for(int j = 0; j<4; j++){
Serial.print(compassVal[i][j]);
Serial.print(" ");
}
Serial.print("\t");
//Serial.print(treasureCount[i]);
Serial.println();
}
reached[nextJunction-1]=1;
Serial.println("Reached");
for(int j = 0; j< 16 ;j++){
Serial.print(reached[j]);
Serial.print(" ");
}
Serial.println();
state = 9;
}
}
else if(result ==2){
lcd.clear();
lcd.print("Parity Error");
totEncorder = totEncorder + knobLeft.read()+knobRight.read();
knobLeft.write(0);
knobRight.write(0);
while(knobLeft.read()+knobRight.read()<totEncorder){
Back(50,50);
}
onPath = 0;
Active_brk(255);
delay(100);
}
}
}
else if(state ==9){
//determine which road to follow next, turn to it
int i;
if(junctionSize[nextJunction-1]==0){
currentDir = 0;
if(reachedBefore ==1){
for(i = 0;i<3;i++){
if(fabs(compassVal[nextJunction-1][currentDir] - compassReading()) > fabs(compassVal[nextJunction-1][i] - compassReading())){
currentDir = i;
}
}
}
else{
currentDir = 2;
}
Serial.println(compassReading());
for(i=0;i<3;i++){
if(covered[nextJunction-1][i] == 0 && neighbours[nextJunction-1][i] !=0){
bestChoice=i;
i++;
break;
}
}
// Serial.print(" BEst 1");
// Serial.println(bestChoice);
// Serial.print(" Current 1 ");
// Serial.println(currentDir);
for(;i<3;i++){
if( covered[nextJunction-1][i] == 0 && treasureCount[neighbours[nextJunction-1][i] -1 ] > treasureCount[neighbours[nextJunction-1][bestChoice] - 1] && neighbours[nextJunction-1][i] !=0){
bestChoice=i;
Serial.print(" Check i 2 ");
Serial.println(treasureCount[neighbours[nextJunction-1][i]]);
Serial.print(" oo Current best 2 ");
Serial.println(treasureCount[neighbours[nextJunction-1][bestChoice]]);
}
}
Serial.print(" BEst 2 ");
Serial.println(bestChoice);
Serial.print(" Current 2 ");
Serial.println(currentDir);
if(bestChoice == 5){
for(i=0;i<3;i++){
if(neighbours[nextJunction-1][i] > 0 ){
bestChoice=i;
break;
i++;
}
}
for(;i<3;i++){
if( reached[neighbours[nextJunction-1][i] - 1] < reached [neighbours[nextJunction-1][bestChoice] -1 ]){
bestChoice=i;
}
}
}
Serial.print(" BEst ");
Serial.println(bestChoice);
Serial.print(" Current ");
Serial.println(currentDir);
lcd.clear();
lcd.print(bestChoice);
lcd.setCursor(0,1);
lcd.print(currentDir);
//turnDirection is 0 for clockwise
//turnDirection is 1 for anticlockwise
turnDirection = -1;
covered[nextJunction-1][bestChoice]=1;
if(bestChoice - currentDir == 1 || (bestChoice == 0 && currentDir == 2)){
turnDirection = 0;
}
else if(bestChoice - currentDir == -1 || (bestChoice == 2 && currentDir == 0)){
turnDirection = 1;
}
if(turnDirection == 0){
while(sensorValuesBool[2] == 1 || sensorValuesBool[3] == 1 || sensorValuesBool[4] == 1 || sensorValuesBool[5] == 1){
antiClockwise(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
while(sensorValuesBool[0] == 0){
antiClockwise(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
while(sensorValuesBool[3] == 0 || sensorValuesBool[4] == 0){
antiClockwise(40);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
}
else if(turnDirection == 1){
while(sensorValuesBool[2] == 1 || sensorValuesBool[3] == 1 || sensorValuesBool[4] == 1 || sensorValuesBool[5] == 1){
clockwise(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
while(sensorValuesBool[0] == 7){
clockwise(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
while(sensorValuesBool[3] == 0 || sensorValuesBool[4] == 0){
clockwise(40);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
}
else if(turnDirection == -1){
}
myservo.attach(10); // attaches the servo on pin 9 to the servo object
myservo.write(4); // sets the servo position according to the scaled value
delay(500); // waits for the servo to get there
myservo.detach();
state =7;
//bestChoice = 5;
previousJunction = nextJunction;
nextJunction = neighbours[previousJunction - 1][bestChoice];
lcd.clear();
lcd.setCursor(0,0);
lcd. print("next");
lcd.setCursor(6,0);
lcd. print(nextJunction);
lcd.setCursor(0,1);
lcd. print("prev");
lcd.setCursor(6,1);
lcd. print(previousJunction);
bestChoice = 5;
}
else if(junctionSize[nextJunction-1]==1){
i = 0;
if(reachedBefore == 1){
currentDir = 0;
for(i = 0;i<4;i++){
if(fabs(compassVal[nextJunction-1][currentDir] - compassReading()) > fabs(compassVal[nextJunction-1][i] - compassReading())){
currentDir = i;
}
}
}
else{
currentDir = 3;
}
//bestChoice=0;
for(i=0;i<4;i++){
if(covered[nextJunction-1][i] == 0 && neighbours[nextJunction-1][i] !=0){
bestChoice=i;
i++;
break;
}
}
for(;i<4;i++){
if(covered[nextJunction-1][i] == 0 && treasureCount[neighbours[nextJunction-1][i] -1]>treasureCount[neighbours[nextJunction-1][bestChoice] -1] && neighbours[nextJunction-1][i] !=0){
bestChoice=i;
}
}
if(bestChoice == 5){
for(i=0;i<4;i++){
if(neighbours[nextJunction-1][i] > 0 ){
bestChoice=i;
i++;
break;
}
}
for(;i<4;i++){
if( neighbours[nextJunction-1][i] !=0 && reached[neighbours[nextJunction-1][i] -1] < reached [neighbours[nextJunction-1][bestChoice]-1]){
bestChoice=i;
}
}
}
lcd.clear();
lcd.print(bestChoice);
lcd.setCursor(0,1);
lcd.print(currentDir);
//turnDirection is 0 for clockwise
//turnDirection is 1 for anticlockwise
//turnDirection is 2 for forward
covered[nextJunction-1][bestChoice]=1;
turnDirection = -1;
if(bestChoice - currentDir == 1 || (bestChoice == 0 && currentDir == 3)){
turnDirection = 0;
}
else if(bestChoice - currentDir == -1 || (bestChoice == 3 && currentDir == 0)){
turnDirection = 1;
}
else if(bestChoice - currentDir == 2 || currentDir - bestChoice == 2){
turnDirection = 2;
}
if(turnDirection == 0){
while(sensorValuesBool[2] == 1 || sensorValuesBool[3] == 1 || sensorValuesBool[4] == 1 || sensorValuesBool[5] == 1){
antiClockwise(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
while(sensorValuesBool[0] == 0){
antiClockwise(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
while(sensorValuesBool[3] == 0 || sensorValuesBool[4] == 0){
antiClockwise(40);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
}
else if(turnDirection == 1){
while(sensorValuesBool[2] == 1 || sensorValuesBool[3] == 1 || sensorValuesBool[4] == 1 || sensorValuesBool[5] == 1){
clockwise(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
while(sensorValuesBool[0] == 7){
clockwise(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
while(sensorValuesBool[3] == 0 || sensorValuesBool[4] == 0){
clockwise(40);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
}
else if(turnDirection == 2){
while(turnDirection > 0){
while(sensorValuesBool[2] == 1 || sensorValuesBool[3] == 1 || sensorValuesBool[4] == 1 || sensorValuesBool[5] == 1){
clockwise(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
while(sensorValuesBool[0] == 7){
clockwise(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
while(sensorValuesBool[3] == 0 || sensorValuesBool[4] == 0){
clockwise(40);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
turnDirection--;
}
Active_brk(255);
delay(100);
}
else if(turnDirection == -1){
}
myservo.attach(10); // attaches the servo on pin 9 to the servo object
myservo.write(4); // sets the servo position according to the scaled value
delay(500); // waits for the servo to get there
myservo.detach();
state =7;
previousJunction = nextJunction;
nextJunction = neighbours[previousJunction - 1][bestChoice];
bestChoice = 5;
}
}
//when a coin is found
else if(state == 10){
linefollowing(0.031,0,0,160,255,10);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
lcd.clear();
lcd.print("state 10");
if(sensorValuesBool[0] ==1){
//coin droping position is in the right hand side
state = 11;
lcd.clear();
lcd.print("right");
}
else if(sensorValuesBool[7] ==1){
//coin droping position is in the left hand side
state = 12;
lcd.clear();
lcd.print("left");
}
}
//coin droping position is in the right hand side
else if(state == 11){
linefollowing(0.031,0,0,160,255,10);
if(analogRead(15) < irThreshold){ //change in black
Active_brk(255);
delay(100);
knobLeft.write(0);
knobRight.write(0);
lcd.clear();
lcd.print("Go forward");
while(knobLeft.read() + knobRight.read() < 5000){
Forwd(40,40);
}
Active_brk(255);
delay(100);
while(sensorValuesBool[2] == 1 || sensorValuesBool[3] == 1 || sensorValuesBool[4] == 1 || sensorValuesBool[5] == 1){
antiClockwise(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
while(sensorValuesBool[0] == 0){
antiClockwise(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
while(sensorValuesBool[3] == 0 || sensorValuesBool[4] == 0){
antiClockwise(40);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
knobLeft.write(0);
knobRight.write(0);
while(knobLeft.read() + knobRight.read() < 1000){/////////////////////////////////////////////
antiClockwise(40);
}
Active_brk(255);
delay(100);
lcd.clear();
lcd.print("Back");
delay(100);
knobLeft.write(0);
knobRight.write(0);
while(knobLeft.read() + knobRight.read() < 4500){
Back(30,30);
}
Active_brk(255);
delay(100);
myservo.attach(10); // attaches the servo on pin 9 to the servo object
myservo.write(60); // sets the servo position according to the scaled value
delay(600); // waits for the servo to get there
myservo.detach();
lcd.clear();
lcd.print("Dropped Coin");
/*if(previousState == 7){
int k;
for(k = 15;k>=0;k--){
if(treasureCount[k] !=-1){
break;
}
}
for(;k>=0;k--){
if(treasureCount[k]!=0){
k=100;
break;
}
}
if(k==-1){
lcd.clear();
lcd.print("Finished");
Active_brk(255);
delay(100);
state = 16;
}
}*/
for(int i =0 ; i<maxJunction;i++){
if(treasureCount[i]!=0){
break;
}
if(i == maxJunction - 1){
lcd.clear();
lcd.print(" All Collected");
Active_brk(255);
delay(100000);
}
}
knobLeft.write(0);
knobRight.write(0);
while(knobLeft.read() + knobRight.read() < 4500){
Forwd(30,30);
}
Active_brk(255);
delay(100);
while(sensorValuesBool[2] == 1 || sensorValuesBool[3] == 1 || sensorValuesBool[4] == 1 || sensorValuesBool[5] == 1){
clockwise(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
while(sensorValuesBool[7] == 0){
clockwise(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
while(sensorValuesBool[3] == 0 || sensorValuesBool[4] == 0){
clockwise(40);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
Active_brk(255);
delay(100);
myservo.attach(10); // attaches the servo on pin 9 to the servo object
myservo.write(4); // sets the servo position according to the scaled value
delay(500); // waits for the servo to get there
myservo.detach();
Serial.print("nextJunction Coins ");
Serial.print(nextJunction);
Serial.print(" ");
Serial.println(treasureCount[nextJunction-1]);
treasureCount[nextJunction-1] = treasureCount[nextJunction-1] - 1;
Serial.print("nextJunction Coins ");
Serial.print(nextJunction);
Serial.print(" ");
Serial.println(treasureCount[nextJunction-1]);
if(previousJunction != 0)
treasureCount[previousJunction-1] = treasureCount[previousJunction-1] -1;
state = previousState;
}
else{
linefollowing(0.031,0,0,160,255,10);
}
}
else if(state == 12){
linefollowing(0.031,0,0,160,255,10);
if(analogRead(8) < irThreshold){
Active_brk(255);
delay(100);
knobLeft.write(0);
knobRight.write(0);
lcd.clear();
lcd.print("Go forward");
while(knobLeft.read() + knobRight.read() < 5000){
Forwd(40,40);
}
Active_brk(255);
delay(100);
while(sensorValuesBool[2] == 1 || sensorValuesBool[3] == 1 || sensorValuesBool[4] == 1 || sensorValuesBool[5] == 1){
clockwise(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
while(sensorValuesBool[7] == 0){
clockwise(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
while(sensorValuesBool[3] == 0 || sensorValuesBool[4] == 0){
clockwise(40);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
knobLeft.write(0);
knobRight.write(0);
while(knobLeft.read() + knobRight.read() < 1000){
clockwise(40);
}
Active_brk(255);
delay(100);
lcd.clear();
lcd.print("Back");
delay(100);
knobLeft.write(0);
knobRight.write(0);
while(knobLeft.read() + knobRight.read() < 4500){
Back(30,30);
}
Active_brk(255);
delay(100);
myservo.attach(10); // attaches the servo on pin 9 to the servo object
myservo.write(60); // sets the servo position according to the scaled value
delay(600); // waits for the servo to get there
myservo.detach();
lcd.clear();
lcd.print("Dropped Coin");
if(previousState == 7){
int k;
for(k = 15;k>=0;k--){
if(treasureCount[k] !=-1){
break;
}
}
for(;k>=0;k--){
if(treasureCount[k]!=0){
k=100;
break;
}
}
if(k==-1){
lcd.clear();
lcd.print("Finished");
Active_brk(255);
delay(100);
state = 16;
}
}
knobLeft.write(0);
knobRight.write(0);
while(knobLeft.read() + knobRight.read() < 4500){
Forwd(30,30);
}
Active_brk(255);
delay(100);
while(sensorValuesBool[2] == 1 || sensorValuesBool[3] == 1 || sensorValuesBool[4] == 1 || sensorValuesBool[5] == 1){
antiClockwise(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
while(sensorValuesBool[0] == 0){
antiClockwise(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
while(sensorValuesBool[3] == 0 || sensorValuesBool[4] == 0){
antiClockwise(40);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
Active_brk(255);
delay(100);
myservo.attach(10); // attaches the servo on pin 9 to the servo object
myservo.write(4); // sets the servo position according to the scaled value
delay(500); // waits for the servo to get there
myservo.detach();
Serial.print("nextJunction Coins ");
Serial.print(nextJunction);
Serial.print(" ");
Serial.println(treasureCount[nextJunction-1]);
treasureCount[nextJunction-1] = treasureCount[nextJunction-1] -1;
Serial.print("nextJunction Coins ");
Serial.print(nextJunction);
Serial.print(" ");
Serial.println(treasureCount[nextJunction-1]);
if(previousJunction != 0)
treasureCount[previousJunction-1] = treasureCount[previousJunction-1];
linefollowing(0.06,0,0,100,255,10);
state = previousState;
}
else{
linefollowing(0.031,0,0,160,255,10);
}
}
}
/*void deadEnd(){
totEncorder = totEncorder + knobLeft.read()+knobRight.read();
Active_brk(255);
delay(100);
knobLeft.write(0);
knobRight.write(0);
compassVal[nextJunction-1][juncCount]=compassReading();
covered[nextJunction-1][juncCount] = 1;
neighbours[nextJunction-1][juncCount] = 0;
juncCount++;
onPath=0;
totEncorder = totEncorder + knobLeft.read()+knobRight.read();
knobLeft.write(0);
knobRight.write(0);
while(knobLeft.read()+knobRight.read()<totEncorder){
Back(50,50);
}
Active_brk(255);
delay(100);
if(((juncCount < 3 && junctionSize[nextJunction -1] == 0) || (juncCount < 4 && junctionSize[nextJunction-1] == 1))){
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
while(sensorValuesBool[3] ==1 || sensorValuesBool[4] ==1){
antiClockwise(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
}
else{
for(int i = 0; i< 16 ; i++){
for(int j = 0; j<4; j++){
Serial.print(neighbours[i][j]);
Serial.print(" ");
}
Serial.print("\t");
Serial.print(treasureCount[i]);
Serial.println();
}
reached[nextJunction-1]=1;
Serial.println("Reached");
for(int j = 0; j< 16 ;j++){
Serial.print(reached[j]);
Serial.print(" ");
}
Serial.println();
state = 9;
}
}*/
void deadEnd2(){
lcd.clear();
lcd.print((knobLeft.read()+knobRight.read()));
while((knobLeft.read()+knobRight.read())<25000){
Forwd(30,30);
}
knobLeft.write(0);
knobRight.write(0);
lcd.clear();
lcd.print("DeadEnd");
while((knobLeft.read()+knobRight.read())<25000){
clockwise(100);
}
lcd.clear();
lcd.print("Pos 2");
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
while(sensorValuesBool[3]==0 && sensorValuesBool[4]==0){
clockwise(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
lcd.clear();
lcd.print("Pos3");
Active_brk(255);
delay(100);
while(sensorCount<5){
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
linefollowing(0.06,0,0,100,255,10);
}
lcd.clear();
lcd.print("Pos4");
while(analogRead(9)>irThreshold && analogRead(14)>irThreshold){
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
Forwd(30,30);
}
knobLeft.write(0);
knobRight.write(0);
while((knobLeft.read()+knobRight.read())<6500){
Forwd(80,80);
}
Active_brk(255);
delay(100);
knobLeft.write(0);
knobRight.write(0);
while((knobLeft.read()+knobRight.read())<36000){
clockwise(100);
}
Active_brk(255);
delay(100);
compassVal[nextJunction-1][juncCount]=compassReading();
covered[nextJunction-1][juncCount] = 1;
neighbours[nextJunction-1][juncCount] = 0;
juncCount++;
onPath=0;
if(((juncCount < 3 && junctionSize[nextJunction -1] == 0) || (juncCount < 4 && junctionSize[nextJunction-1] == 1))){
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
while(sensorValuesBool[3] ==1 || sensorValuesBool[4] ==1){
antiClockwise(100);
position = qtrrc.readLine(sensorValues,1,1);
setSensorValues();
}
}
else{
for(int i = 0; i< 16 ; i++){
for(int j = 0; j<4; j++){
Serial.print(neighbours[i][j]);
Serial.print(" ");
}
Serial.print("\t");
Serial.print(treasureCount[i]);
Serial.println();
}
reached[nextJunction-1]=1;
Serial.println("Reached");
for(int j = 0; j< 16 ;j++){
Serial.print(reached[j]);
Serial.print(" ");
}
Serial.println();
state = 9;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment