donmike25 icon

test code motor n IR

donmike25 | PRO | 01/31/16 07:53:52 PM UTC | 0 ⭐ | 191 👁️ | Never ⏰ | []
text |

2.67 KB

|

None

|

0 👍

/

0 👎

#define leftp 7
#define leftn 4
#define rightp 3
#define rightn 2
#define speedleft 5
#define speedright 6
#define surfaceSense1 9
#define surfaceSense2 8
 //Function prototypes
 void straight(int speedpwm);
void left(int speedpwm);
void right(int speedpwm);
void reverse(int speedpwm);
void brake(int speedpwm);
void readSurface();
 void setup() {
  Serial.begin(9600);
  pinMode(leftp, OUTPUT);
  pinMode(leftn, OUTPUT);
  pinMode(rightp, OUTPUT);
  pinMode(rightn, OUTPUT);
  pinMode(speedleft, OUTPUT);
  pinMode(speedright, OUTPUT);
  pinMode(surfaceSense1, INPUT);
  pinMode(surfaceSense2, INPUT);
  void straight(int speedpwm){
  digitalWrite(leftp, HIGH);
  digitalWrite(leftn, LOW);
  digitalWrite(rightp, HIGH);
  digitalWrite(rightn, LOW);
  analogWrite(speedleft, speedpwm);
  analogWrite(speedright, speedpwm); 
  }
 void left(int speedpwm){
  digitalWrite(leftp, LOW);
  digitalWrite(leftn, LOW);
  digitalWrite(rightp, HIGH);
  digitalWrite(rightn, LOW);  
  analogWrite(speedleft, speedpwm);
  analogWrite(speedright, speedpwm); 
  }
 void right(int speedpwm){
  digitalWrite(leftp, HIGH);
  digitalWrite(leftn, LOW);
  digitalWrite(rightp, LOW);
  digitalWrite(rightn, LOW);
  analogWrite(speedleft, speedpwm);
  analogWrite(speedright, speedpwm); 
  }
 void reverse(int speedpwm){
  digitalWrite(leftp, LOW);
  digitalWrite(leftn, HIGH);
  digitalWrite(rightp, LOW);
  digitalWrite(rightn, HIGH);
  analogWrite(speedleft, speedpwm);
  analogWrite(speedright, speedpwm); 
  }
void brake(int speedpwm){
  digitalWrite(leftp, LOW);
  digitalWrite(leftn, LOW);
  digitalWrite(rightp, LOW);
  digitalWrite(rightn, LOW);
  analogWrite(speedleft, speedpwm);
  analogWrite(speedright, speedpwm); 
  }
 void readSurface(){
  //For left sensor
  if(digitalRead(surfaceSense1) == HIGH){
    //Check if both sensor was triggered simultaneously
    if(digitalRead(surfaceSense2) == HIGH){
      //Robot backs up for a bit before making appropriate turning
      reverse(255);
      delay(1000); 
      }
    //Here the robot avoids the line from the left
    right(255);
    delay(500);
    //Keeps executing until the line was cleared from sensor
    readSurface();
    }
  //For right sensor
  else if(digitalRead(surfaceSense2) == HIGH){
    //Check if both sensor was triggered simultaneously
    if(digitalRead(surfaceSense2) == HIGH){
      //Robot backs up for a bit before making appropriate turning
      reverse(255);
      delay(1000); 
      }
    //Here the robot avoids the line from the right
    left(255);
    delay(500);
    //Keeps executing until the line was cleared from sensor
    readSurface();
  }

Comments