SHARE
TWEET

Untitled

zeeph Dec 20th, 2018 55 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #define MeMCore_H  
  2. #include <Makeblock.h>
  3. #include <SoftwareSerial.h>
  4.  
  5. MeDCMotor MotorL(M1);  
  6. MeDCMotor MotorR(M2);
  7. MeLineFollower lineFollower(PORT_4); //line follower
  8. MeUltrasonicSensor ultraSensor(PORT_2); //ultrasonicSensor
  9.  
  10. uint8_t moveSpeed = 255;
  11. double lineDirIndex= 10;
  12. double dist;
  13.  
  14. void setup() {
  15.   // put your setup code here, to run once:
  16.   Serial.begin(9600);
  17. }
  18.  
  19. void loop() {
  20.   // put your main code here, to run repeatedly:
  21.   dist = ultraSensor.distanceCm();
  22.   Serial.println(dist);
  23.  
  24.   if(dist<12 && dist>1)
  25.   {
  26.     obstacleAvoidance();      
  27.   } else {
  28.     lineFollow();  
  29.   }
  30. }
  31.  
  32. void lineFollow(){
  33.   int sensorStateCenter = lineFollower.readSensors();
  34.  
  35.   if(moveSpeed>255) {
  36.     moveSpeed=255;
  37.   }
  38.   switch(sensorStateCenter)
  39.   {
  40.     case S1_IN_S2_IN:
  41.       //Serial.println("Sensor 1 and 2 are inside of black line");
  42.       Forward(); // Forward    
  43.       lineDirIndex=10;
  44.       break;
  45.     case S1_IN_S2_OUT:
  46.       //Serial.println("Sensor 2 is outside of black line");
  47.       Forward(); // Forward    
  48.       if(lineDirIndex>1) {
  49.         lineDirIndex--;
  50.       }      
  51.       break;
  52.     case S1_OUT_S2_IN:
  53.       //Serial.println("Sensor 1 is outside of black line");
  54.       Forward(); // Forward
  55.       if(lineDirIndex<20) {
  56.         lineDirIndex++;
  57.       }    
  58.       break;
  59.     case S1_OUT_S2_OUT:
  60.       //Serial.println("Sensor 1 and 2 are outside of black line");
  61.       if(lineDirIndex==10){
  62.       Backward(); // Backward
  63.       }
  64.       if(lineDirIndex==10.5){
  65.       MotorL.run(-moveSpeed); // Turn right
  66.       MotorR.run(moveSpeed/3); //ici
  67.       }
  68.       if(lineDirIndex<10.5){
  69.         TurnLeft(); // Turn left
  70.       }
  71.       if(lineDirIndex>10.5){
  72.         TurnRight(); // Turn right
  73.       }            
  74.       break;
  75.   }
  76. }
  77.  
  78. void obstacleAvoidance() {
  79.   MotorL.run(moveSpeed); // Turn left
  80.   MotorR.run(moveSpeed);
  81.   delay(400);
  82.   lineDirIndex=10.5;
  83. }
  84.  
  85. void Forward()
  86. {
  87.   MotorL.run(-moveSpeed);
  88.   MotorR.run(moveSpeed);
  89. }
  90. void Backward()
  91. {
  92.   MotorL.run(moveSpeed);
  93.   MotorR.run(-moveSpeed);
  94. }
  95. void TurnLeft()
  96. {
  97.   MotorL.run(-moveSpeed/20); // Turn left //ici
  98.   MotorR.run(moveSpeed+10);
  99. }
  100. void TurnRight()
  101. {
  102.   MotorL.run(-moveSpeed);// Turn right
  103.   MotorR.run(moveSpeed/20); //ici
  104. }
  105. void Stop()
  106. {
  107.   MotorL.run(0);
  108.   MotorR.run(0);
  109. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top