Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- int motors [] = {5,6,10,11};
- int inputPins [] = {A1,A2,A5}; //0 - center, 1 - right, 2 - left --------- Left,
- int inputVal [] = {0,0,0};
- int centerWhite = 800;
- int leftWhite = 765;
- int rightWhite = 635;
- int whiteCenter = 860;
- int whites[] = {900, 750, 765};
- int defWhite = 750;
- int readSpeed = 15;
- int dutyCycle = 65;
- int turnCount = 0;
- int interTurnTimer = 1050;
- int allWhiteForward = 900;
- boolean spinOnce = 0;
- unsigned long timer;
- /*
- Center: white - 980, black - 740, threshold - 860
- Right: white - 830, black - 440, threshold - 635
- Left: white - 880, black - 650, threshold - 765
- */
- //left - A3, center - A1, right - A2
- void setup (){
- Serial.begin (9600);
- for (int i = 0; i < 3; i++)
- {
- pinMode (motors [i], OUTPUT);
- }
- }
- void loop (){
- Serial.println ((inputVal [0])+100); //center
- Serial.println ((inputVal [1])+100); //right
- Serial.println ((inputVal [2])); //left
- Serial.println();
- if(inputVal[0]+100 <= whites[0])
- {
- turnCount = 0;
- spinOnce = 0;
- if(inputVal[1]+100 <= defWhite && inputVal[0]+100 <= defWhite)
- {
- //go right if at intersection
- Serial.println("Intersection");
- timer = millis ();
- move (1);
- delay (35h0);
- while (true)
- {
- move(5);
- Serial.println("inter turn");
- Serial.println ((inputVal [0])+100); //center
- Serial.println ((inputVal [1])+100); //right
- Serial.println ((inputVal [2])); //left
- Serial.println();
- delay (readSpeed);
- for (int i = 0; i < 3; i++)
- {
- inputVal [i] = analogRead (inputPins [i]);
- }
- if (millis () - timer > interTurnTimer && inputVal [0] <= whites [0]){
- break;
- }
- }
- }
- else
- {
- spinOnce = 0;
- //go forward
- Serial.println("Forward");
- move (1);
- for (int i = 0; i < 3; i++){
- inputVal [i] = analogRead (inputPins [i]);
- }
- }
- }
- else if(inputVal [1]+100 <= whites[1]){
- turnCount = 0;
- spinOnce = 0;
- //go right
- Serial.println("Right");
- move (3);
- for (int i = 0; i < 3; i++){
- inputVal [i] = analogRead (inputPins [i]);
- }
- }else if(inputVal [2] <= whites[2]){
- turnCount = 0;
- spinOnce = 0;
- //go left
- Serial.println("Left");
- move (4);
- for (int i = 0; i < 3; i++){
- inputVal [i] = analogRead (inputPins [i]);
- }
- }else if(inputVal[0]+100 >= whites[0] && inputVal [1]+100 >= whites[1] && inputVal[2] >= whites[2]){
- //go left if all white
- turnCount++;
- if (turnCount >= 3)
- {
- if (spinOnce = 0){
- Serial.println ("Move for white");
- timer = millis ();
- move (1);
- delay (300);
- spinOnce = 1;
- }
- while (true)
- {
- Serial.println("All white. Left");
- Serial.println ((inputVal [0])+100); //center
- Serial.println ((inputVal [1])+100); //right
- Serial.println ((inputVal [2])); //left
- Serial.println ();
- move (6);
- for (int i = 0; i < 3; i++)
- {
- inputVal [i] = analogRead (inputPins [i]);
- }
- if (inputVal [0]+100 <= whites [0] || inputVal[1]+100 <= whites[1] || inputVal[2] <= whites[2]){
- break;
- }
- }
- }
- } else{
- Serial.println("no scenario found");
- move (1);
- for (int i = 0; i < 3; i++){
- inputVal [i] = analogRead (inputPins [i]);
- }
- }
- delay(readSpeed);
- }
- //Sets movement direction.
- //1 is forwards | 2 is backwards | 3 is turn right | 4 is turn left
- void move (int direction){
- if(direction == 1){
- analogWrite (motors [0], dutyCycle);
- analogWrite (motors [1], LOW);
- analogWrite (motors [2], dutyCycle);
- analogWrite (motors [3], LOW);
- }
- else if(direction == 2){
- analogWrite (motors [0], LOW);
- analogWrite (motors [1], dutyCycle);
- analogWrite (motors [2], LOW);
- analogWrite (motors [3], dutyCycle);
- }
- else if(direction == 3){
- analogWrite (motors [0], dutyCycle - 15);
- analogWrite (motors [1], LOW);
- analogWrite (motors [2], LOW);
- analogWrite (motors [3], LOW);
- }
- else if(direction == 4){
- analogWrite (motors [0], LOW);
- analogWrite (motors [1], LOW);
- analogWrite (motors [2], dutyCycle-10);
- analogWrite (motors [3], LOW);
- }else if(direction == 5){
- analogWrite (motors [0], dutyCycle-5); //+15
- analogWrite (motors [1], LOW);
- analogWrite (motors [2], LOW);
- analogWrite (motors [3], dutyCycle-5); //-25
- }else if(direction == 6){
- analogWrite (motors [0], LOW);
- analogWrite (motors [1], dutyCycle-20);
- analogWrite (motors [2], dutyCycle-20);
- analogWrite (motors [3], LOW);
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement