Maze Solving Problem

Projects Programming
1

I have a project that have to make a Maze Solver using 5 sensor and 2 motor . I have code it but when I upload the right motor keep spinning but the left motor is not moveing , when any of the sensor is on the white line it just stop.

#include<util/delay.h>
int LeftOutSensor; // val for Sensor 1 P2
int LeftCentreSensor; // val for Sensor 2 P3
int val3; // val for Sensor 3 P4
int RightCentreSensor; // val for Sensor 4 P5
int RightOutSensor; // val for Sensor 5 P6
int leftNudge;
int replaystage;
int rightNudge;
char path[30] = {};
int pathLength;
int readLength;

void setup()
{
    DDRE &= ~_BV(DDE4);   // Make Pin 2 PE4 as an input Sensor 1
    DDRE &= ~_BV(DDE5);   // Make Pin 3 PE5 as an input Sensor 2
    DDRG &= ~_BV(DDG5);   // Make Pin 4 PG5 as an input Sensor 3
    DDRE &= ~_BV(DDE3);   // Make Pin 5 PE3 as an input Sensor 4
    DDRH &= ~_BV(DDH3);   // Make Pin 6 PH3 as an input Sensor 5
    
    DDRB = 0x30;        //Make PB4 PB5 as an OUTPUT Pin 10 Pin 11 for LeftMotor
    DDRH = 0x60;        //Make PH5 PH6 as an OUTPUT Pin 8 Pin 9 For RightMotor
    DDRB & 0b10000000;      //Make PB7 an output pin
    _delay_ms(1000);
}
void loop(){
  readSensors();
  if(LeftOutSensor>4 && RightOutSensor>4 && (LeftCentreSensor >4 || RightCentreSensor>4)) //If we do not see a left or right turn and at least 1 of our mid sensor see
  {                                           // the black line , keep going straight
    straight();
  }
  else{
    leftHandWall();
  }
}

void readSensors(){
    LeftOutSensor = PINE & _BV(PE4);
    LeftCentreSensor = PINE & _BV(PE5);
    val3 = PING & _BV(PG5);
    RightCentreSensor = PINE & _BV(PE3);
    RightOutSensor = PINH & _BV(PH3);
}

void leftHandWall(){
 
  if(LeftOutSensor<4 && RightOutSensor<4){ // If S1 and S5 are on the black line
   HLHL();
  _delay_ms(250);
  readSensors();  //recheck the sensor
  if(LeftOutSensor<4 || RightOutSensor<4){  // if S1 and S5 are on the white
    done();
  }
  if(LeftOutSensor>4 && RightOutSensor>4){ // If both S1 and S5 are on the white which mean need to turn left
    turnLeft();
  }
}
if (LeftOutSensor < 4 )
{
   HLHL();
  _delay_ms(250);
  readSensors();
  if(LeftOutSensor>4 && RightOutSensor>4) // both S1 S5 on white turn left
  {
    turnLeft();
  }
  else
  {
    done();
  }
}

if(RightOutSensor<4)
{
_delay_ms(10);
  readSensors;
  if(LeftOutSensor<4)
  {
    _delay_ms(240);
    readSensors;
    if(RightOutSensor<4 && LeftOutSensor<4) // if all are Black
   {
    done();
   }
   else
  {
   turnLeft();
  return;
  }
  }
  _delay_ms(240);
readSensors();
if(LeftOutSensor>4 && LeftCentreSensor<4 && RightCentreSensor<4 && RightOutSensor>4) //all are white right turn
{
  turnRight();
  return;
}
    path[pathLength]='S';
    pathLength++;
    if(path[pathLength-2]=='B')
    {
      shortPath();   
    } 
    straight();
}
readSensors();
if(LeftOutSensor>4 && LeftCentreSensor<4 && val3 <4 && RightCentreSensor<4 && RightOutSensor>4)
{
  turnAround();
}
}
void done()
{
  LLLL();
  replaystage=1;
  path[pathLength]='D';
  pathLength++;
  while(LeftOutSensor<4){
  PORTB = PORTB | 0b10000000;              //Set PB7 to turn on LED
  _delay_ms(150);
  PORTB = PORTB & 0b01111111;              //Clear PB7 to turn off LED
  _delay_ms(150);
}
 _delay_ms(1000);

replay();
}

void turnLeft()
{
  while(RightCentreSensor>4 || LeftCentreSensor>4){
  LHHL();
  _delay_ms(500);
  LLLL();
  delay(250);
  }
  while(RightCentreSensor<4){
    LHHL();
    _delay_ms(500);
    LLLL();
    _delay_ms(250);
  }

  
   if(replaystage==0){
    path[pathLength]='L';
    pathLength++;
      if(path[pathLength-2]=='B'){
        shortPath();
      }
  }
}

void turnRight()
{
  while(RightOutSensor>4)
  {
    HLLH();
    _delay_ms(500);
    LLLL();
    _delay_ms(250);
  }
  while(RightCentreSensor<4)
  {
    HLLH();
    _delay_ms(500);
    LLLL();
    _delay_ms(250);
  }
    if(replaystage==0){
  path[pathLength]='R';
  pathLength++;
    if(path[pathLength-2]=='B'){
      shortPath();
    }
  }
  straight();
 
}

void straight(){

  if(LeftCentreSensor<4)
  {
    HLHL();
    _delay_ms(200); 
    HLLL();
    _delay_ms(250);
  }
  if(RightCentreSensor<4)
  {
    HLHL();
    _delay_ms(200);
    LLHL();
    _delay_ms(250);
  }
else
{
  HLHL();
}
}
void turnAround()
{
  HLHL();
  _delay_ms(250);
  while(LeftOutSensor>4)
  {
    LHHL();
    _delay_ms(250);
    LLLL();
    _delay_ms(250);
  }
  while(LeftCentreSensor<4){
    LHHL();
  }
    
   path[pathLength]='B';
  pathLength++;
  straight();  
}
  
void shortPath()
{
  int shortDone=0;
  if(path[pathLength-3]=='L' && path[pathLength-1]=='R')
  {
    pathLength-=3;
    path[pathLength]='B';
 
    shortDone=1;
  } 
  if(path[pathLength-3]=='L' && path[pathLength-1]=='S' && shortDone==0)
  {
    pathLength-=3;
    path[pathLength]='R';
 
    shortDone=1;
  } 
  if(path[pathLength-3]=='R' && path[pathLength-1]=='L' && shortDone==0)
  {
    pathLength-=3;
    path[pathLength]='B';

    shortDone=1;
  } 
  if(path[pathLength-3]=='S' && path[pathLength-1]=='L' && shortDone==0)
  {
    pathLength-=3;
    path[pathLength]='R';

    shortDone=1;
  }   
  if(path[pathLength-3]=='S' && path[pathLength-1]=='S' && shortDone==0)
  {
    pathLength-=3;
    path[pathLength]='B';
    shortDone=1;
  }
  if(path[pathLength-3]=='L' && path[pathLength-1]=='L' && shortDone==0)
  {
    pathLength-=3;
    path[pathLength]='S';
    shortDone=1;
  }
  path[pathLength+1]='D';
  path[pathLength+2]='D';
  pathLength++;
}

void replay()
{
  readSensors();
  if(LeftOutSensor>4 && RightOutSensor>4)
{
  straight();
}
else
{
  if(path[readLength]=='D')
  {
    HLHL();
    _delay_ms(250);
    LLLL();
    endMotion();
  }
   if(path[readLength]=='L')
    {
      HLHL();
      _delay_ms(250);
      turnLeft();
    }
    if(path[readLength]=='R')
    {
      HLHL();
      _delay_ms(250);
      turnRight();
    }
    if(path[readLength]=='S')
    {
      HLHL();
      _delay_ms(250);
      straight();
    }
    readLength++;
  }
  replay();
}

void endMotion()
{
  PORTB = PORTB | 0b10000000;              //Set PB7 to turn on LED
  _delay_ms(500);
  PORTB = PORTB & 0b01111111;              //Clear PB7 to turn off LED
  _delay_ms(200);
  PORTB = PORTB | 0b10000000;              //Set PB7 to turn on LED
  _delay_ms(200);
  PORTB = PORTB & 0b01111111;              //Clear PB7 to turn off LED
  _delay_ms(500);
}

void HLHL()
{
          PORTB &= 0xCF;        // Make PB4=0 and PB5=0 FF
          PORTB |= 0x20;        // on the line, roatate motor clockwise PB5=1, PB4=0 OF
          PORTH &= 0x9F;        //Make PH6=0 and PH5=0  FF
          PORTH |= 0x40;        //Not on the line, rotate motor clockwise PH6=1 and PH5=0 OF
          
}

void LLLL()
{
          PORTB &= 0xCF;        // Make PB4=0 and PB5=0 FF
          PORTB |= 0xCF;        // stop motor
          PORTH &= 0x9F;        //Make PH6=0 and PH5=0  FF
          PORTH |= 0x9F;        // stop motor
          
}
void HLLH()
{
          PORTB &= 0xCF;        // Make PB4=0 and PB5=0 FF
          PORTB |= 0x20;        // on the line, roatate motor clockwise PB5=1, PB4=0 OF
          PORTH &= 0x9F;        //Make PH6=0 and PH5=0  FF
          PORTH |= 0x20;        //Not on the line, rotate motor anticlockwise PH6=0 and PH5=1 OF
          
}
void LHHL()
{
          PORTB &= 0xCF;        // Make PB4=0 and PB5=0 FF
          PORTB |= 0x10;        // on the line, roatate motor anticlockwise PB5=0, PB4=1 OF
          PORTH &= 0x9F;        //Make PH6=0 and PH5=0  FF
          PORTH |= 0x40;        //Not on the line, rotate motor clockwise PH6=1 and PH5=0 OF
          
}
void LLHL()
{
          PORTB &= 0xCF;        // Make PB4=0 and PB5=0 FF
          PORTB |= 0xCF;        // stop motor
          PORTH &= 0x9F;        //Make PH6=0 and PH5=0  FF
          PORTH |= 0x40;        //Not on the line, rotate motor anticlockwise PH6=1 and PH5=0 OF
}
void HLLL()
{
          PORTB &= 0xCF;        // Make PB4=0 and PB5=0 FF
          PORTB |= 0x20;        // on the line, roatate motor clockwise PB5=1, PB4=0 OF
          PORTH &= 0x9F;        //Make PH6=0 and PH5=0  FF
          PORTH |= 0x9F;        // stop motor
}

Please help me , im a noob at this stuff . Thanks for the help

I have test single code in it like void turnLeft , void turnLeft, void turnRight, void Straight...etc
But when put them up together like that it doesnt work .
I think is a logical problem

2 
    DDRE &= ~_BV(DDE4);   // Make Pin 2 PE4 as an input Sensor 1
    DDRE &= ~_BV(DDE5);   // Make Pin 3 PE5 as an input Sensor 2
    DDRG &= ~_BV(DDG5);   // Make Pin 4 PG5 as an input Sensor 3
    DDRE &= ~_BV(DDE3);   // Make Pin 5 PE3 as an input Sensor 4
    DDRH &= ~_BV(DDH3);   // Make Pin 6 PH3 as an input Sensor 5
    
    DDRB = 0x30;        //Make PB4 PB5 as an OUTPUT Pin 10 Pin 11 for LeftMotor
    DDRH = 0x60;        //Make PH5 PH6 as an OUTPUT Pin 8 Pin 9 For RightMotor
    DDRB = 0b10000000;      //Make PB7 an output pin
    _delay_ms(1000);

This is nonsense. The use of direct port manipulation makes sense when speed is important. But, if speed WERE important, you would NOT have a delay() call anywhere in the code. Use pinMode() and digitalWrite().

3

im a noob at this stuff

Yet, you wrote code embracing sophisticated concepts with port manipulation in there, so I'm surprised you can't debug it yourself. It's certainly beyond my capabilities.

4 
    DDRB = 0x30;        //Make PB4 PB5 as an OUTPUT Pin 10 Pin 11 for LeftMotor
    DDRH = 0x60;        //Make PH5 PH6 as an OUTPUT Pin 8 Pin 9 For RightMotor
    DDRB = 0b10000000;      //Make PB7 an output pin

When you set DDRB to 0b10000000, it overrides the value of 0x30 that you set earlier.

Hint: avoid use of "=" to set register values, use "|" or "&" to set or clear bits.

5 
    val1 = PINE & _BV(PE4);
    val2 = PINE & _BV(PE5);
    val3 = PING & _BV(PG5);
    val4 = PINE & _BV(PE3);
    val5 = PINH & _BV(PH3);
}

void leftHandWall(){
  if(val1<4 && val5<4){ // If S1 and S5 are on the black line

This stuff is very confusing. Just do digitalRead of the appropriate pins, which you have as constants at the start of the sketch.

And name things logically. Not val1, val2 etc. but leftSensor, middleSensor, etc.

6

I know the I/O is very confusing but my program cannot use any Arduino Library ( stupid teacher) .
And think you are right jremington ill try to get that out , i think that why my left motor is not working .
BTW
I have 5 sensor i bought it 2 different time first time i bought 2 then second time i bought 3 .
First 2 Sensor ( Sensor 1 and Sensor 5) : When it on black it give me 0, when it on white it give me 16
Second 3 Sensor( S2 S3 S4) : When it on black it give me 16 , when it on white it give me 0

I wish i can control the speed of the motor using PWM , i do a lots of research still cant find any PWM tut that show u how to control a motor speed without Arduino library. My motor speed run like crazy and throw the robot out of the maze , the only solution atm is use small voltage batteries . :frowning:

SADDDDDDDDDDD :frowning:

BTW jreminton thanks for that it just make my left motor move now and the LED!# still working by using the "&" .
Can u have a look at the end of the code , the way i set my motor using "&=" and "|=" is there anything wrong there cuz it still not working probably as i want ,

7

When I read the title I had assumed the maze was external to the robot - but it seems to be internal.

i do a lots of reserch still cant find any PWM tut that show u how to control a motor speed without Arduino library.

The Arduino PWM code would be good tutorial.

...R

8

chasingsun360:
( stupid teacher) .

A bad workman blames his tools.

9

Why are you using the Arduino if you are not allowed to use the libraries?

Get your teacher to post here and we will have a nice chat.

10

chasingsun360:

#include<util/delay.h>

I notice you are allowed to use this library.

11

Haha u are right , cuz the Course have 50 student and 40 of them still dont now how to use I/O , PWM , ..etc.
I understand that is teach them to fish not give them the fish , But is way to hard when people already design a nice library such as Arduino Library and u can use it make the robot way easier ??? Working smart not working hard ... ( but i dumb)

Why u try to make it complicate ?

Well i agree with you im a bad workman :slight_smile:

12

I wish i can control the speed of the motor using PWM , i do a lots of research still cant find any PWM tut that show u how to control a motor speed without Arduino library.

Does this......

analogWrite(somePWMPinOrOther, aNiceSpeed);

.... not work for you?

NLR.... No Library Required.

40 of them still dont now how to use ... PWM

Clearly you're one of them.

13

I don't agree with this idea of not using libraries. You may as well say don't use compilers. Get your teacher to get in touch if he (or she) wants to discuss this.

14

Exactly , why using Arduino Mega 2560 and dont let us used the Arduino Library ...??? Super question!!! i've been asking that question in my head.

Yeah we are allow to use AVR library the <util/delay.h> is avr library .
Here come another question ? Why u can use the AVR library but not the Arduino Library .... hmmmm life is hard!

15 
analogWrite(somePWMPinOrOther, aNiceSpeed);

.... not work for you?

NLR.... No Library Required.

40 of them still dont now how to use ... PWM

Clearly you're one of them.
[/quote]

analogWrite is arduino library right ?
yeah I am one of them
:))

16

OK.

@Teacher: Not letting them use the Arduino libraries is just encouraging them to write unreadable code. You don't want that, do you? The real test is making things work properly. The library is just a tool. A good programmer will make a good result, even using the libraries. With luck, they will even learn the limitations that libraries impose (if any). A bad programmer won't get a good result, even if you give them 100 libraries and 100 books to read. Part of learning to program is learning to use the available tools properly.

17

Yeah maybe u are right because you are a teacher as well i guess :slight_smile: , Good training i agree cuz myself have to do alot of research to get that far . But we are in year 11 :frowning:

Do anyone know how to use PWM without Arduino Library to control the motor speed .???

18

analogWrite is arduino library right ?

Are you actually saying that you may not use any of these commands here? Well you already used setup() and loop().... to me analogWrite's no different from them.

edit.... or does s/he mean that you're allowed to use those commands but not any other libraries that you need to declare at the top, like for LCDs and RTCs and whatnot?

19

yeb , can not use any code form Arduino Library including the analogWrite() yes yes yes

the setup() and the loop() exclude maybe hahaha

edit.... and other library as well including the one LEDS.h...etc
can use AVR library , C library ( stdio.h) ( conio.h) ...etc

20

I'd be very keen to see your lecturer's problem statement to your class.

the setup() and the loop() exclude maybe hahaha

Only maybe?- perhaps s/he expects you to code that long hand too.

edit... what about "if"? You used that. Is that ok because it's "pure C" not an "Arduino extension" (for want of a better word).