Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <SoftwareSerial.h>
- bool autoDriveCheck = false;
- const int inputMidLeft = 12;
- const int inputMidRight = 8;
- const int inputFarLeft = 13;
- const int inputFarRight = 7;
- bool midLeftVal;
- bool midRightVal;
- bool farLeftVal;
- bool farRightVal;
- //definations af motorenes pins
- const int motor1A = 11;
- const int motor1B = 10;
- const int motor2A = 6;
- const int motor2B = 5;
- const int Gun = 4;
- int BluetoothTX = 0;
- int BluetoothRX = 1;
- SoftwareSerial bluetooth(BluetoothTX, BluetoothRX);
- String state;
- int verdi = 0;
- void setup() {
- // put your setup code here, to run once:
- pinMode(motor1A, OUTPUT);
- pinMode(motor1B, OUTPUT);
- pinMode(motor2A, OUTPUT);
- pinMode(motor2B, OUTPUT);
- //senscore
- pinMode(inputMidLeft, INPUT);
- pinMode(inputMidRight, INPUT);
- pinMode(inputFarLeft, INPUT);
- pinMode(inputFarRight, INPUT);
- pinMode(Gun, OUTPUT);
- bluetooth.begin(115200);
- Serial.begin(115200);
- autoDriveCheck = false;
- }
- void loop() {
- // put your main code here, to run repeatedly:
- //Opdater bluetooth inputtet hvis en bluetooth input er givet.
- while (bluetooth.available()>0){
- delay(10);
- char c = bluetooth.read();
- state += c;
- }
- //Check om autodrive værdien er ændre og store det i en boolean værdi. Hvis ikke, fortsæt.
- if (state = "A1"){
- autoDriveCheck = true;
- }
- else if (state = "A0")
- autoDriveCheck = false;
- if (autoDriveCheck = true)
- {
- //senscore
- midLeftVal = digitalRead(inputMidLeft);
- midRightVal = digitalRead(inputMidRight);
- farLeftVal = digitalRead(inputFarLeft);
- farRightVal = digitalRead(inputFarRight);
- verdi = farLeftVal*1+midLeftVal*2+midRightVal*4+farRightVal*8;
- switch (verdi) {
- case 15:
- //bak
- analogWrite (motor1A, 0);
- analogWrite (motor1B, 255);
- analogWrite (motor2A, 0);
- analogWrite (motor2B, 255);
- delay(0100);
- break;
- /*case 6:
- //Frem
- analogWrite (motor1A, 255);
- analogWrite (motor1B, 0);
- analogWrite (motor2A, 255);
- analogWrite (motor2B, 0);
- break;*/
- case 4:
- //meget skarpt right
- analogWrite (motor1A, 0);
- analogWrite (motor1B, 255);
- analogWrite (motor2A, 255);
- analogWrite (motor2B, 0);
- break;
- case 2:
- //meget skarpt left
- analogWrite (motor1A, 255);
- analogWrite (motor1B, 0);
- analogWrite (motor2A, 0);
- analogWrite (motor2B, 255);
- break;
- case 0:
- //Frem
- analogWrite (motor1A, 255);
- analogWrite (motor1B, 0);
- analogWrite (motor2A, 255);
- analogWrite (motor2B, 0);
- break;
- case 9:
- //Frem
- analogWrite (motor1A, 255);
- analogWrite (motor1B, 0);
- analogWrite (motor2A, 255);
- analogWrite (motor2B, 0);
- break;
- case 5:
- //frem
- analogWrite (motor1A, 255);
- analogWrite (motor1B, 0);
- analogWrite (motor2A, 255);
- analogWrite (motor2B, 0);
- break;
- case 10:
- //frem
- analogWrite (motor1A, 255);
- analogWrite (motor1B, 0);
- analogWrite (motor2A, 255);
- analogWrite (motor2B, 0);
- break;
- case 3:
- //Right
- analogWrite (motor1A, 255);
- analogWrite (motor1B, 0);
- analogWrite (motor2A, 190);
- analogWrite (motor2B, 0);
- break;
- case 11:
- //Right
- analogWrite (motor1A, 255);
- analogWrite (motor1B, 0);
- analogWrite (motor2A, 190);
- analogWrite (motor2B, 0);
- break;
- case 13:
- //Left
- analogWrite (motor1A, 190);
- analogWrite (motor1B, 0);
- analogWrite (motor2A, 255);
- analogWrite (motor2B, 0);
- break;
- case 12:
- //Left
- analogWrite (motor1A, 190);
- analogWrite (motor1B, 0);
- analogWrite (motor2A, 255);
- analogWrite (motor2B, 0);
- break;
- case 7:
- //Sharp Right
- analogWrite (motor1A, 255);
- analogWrite (motor1B, 0);
- analogWrite (motor2A, 0);
- analogWrite (motor2B, 0);
- break;
- case 1:
- //Sharp Right
- analogWrite (motor1A, 255);
- analogWrite (motor1B, 0);
- analogWrite (motor2A, 0);
- analogWrite (motor2B, 0);
- break;
- case 14:
- //Sharp Left
- analogWrite (motor1A, 0);
- analogWrite (motor1B, 0);
- analogWrite (motor2A, 255);
- analogWrite (motor2B, 0);
- break;
- case 8:
- //Sharp Left
- analogWrite (motor1A, 0);
- analogWrite (motor1B, 0);
- analogWrite (motor2A, 255);
- analogWrite (motor2B, 0);
- break;
- }}
- else {
- if (state.length() > 0) {
- Serial.println(state );
- if(state == "a")
- {
- analogWrite (motor1A, 255);
- analogWrite (motor1B, 0);
- analogWrite (motor2A, 255);
- analogWrite (motor2B, 0);
- }
- else if(state == "b")
- {
- analogWrite (motor1A, 0);
- analogWrite (motor1B, 0);
- analogWrite (motor2A, 0);
- analogWrite (motor2B, 0);
- }
- if(state == "c")
- {
- analogWrite (motor1A, 0);
- analogWrite (motor1B, 0);
- analogWrite (motor2A, 255);
- analogWrite (motor2B, 0);
- }
- else if(state == "d")
- {
- analogWrite (motor1A, 0);
- analogWrite (motor1B, 0);
- analogWrite (motor2A, 0);
- analogWrite (motor2B, 0);
- }
- if(state == "e")
- {
- analogWrite (motor1A, 255);
- analogWrite (motor1B, 0);
- analogWrite (motor2A, 0);
- analogWrite (motor2B, 0);
- }
- else if(state == "f")
- {
- analogWrite (motor1A, 0);
- analogWrite (motor1B, 0);
- analogWrite (motor2A, 0);
- analogWrite (motor2B, 0);
- }
- if(state == "g")
- {
- analogWrite (motor1A, 0);
- analogWrite (motor1B, 255);
- analogWrite (motor2A, 0);
- analogWrite (motor2B, 255);
- }
- else if(state == "h")
- {
- analogWrite (motor1A, 0);
- analogWrite (motor1B, 0);
- analogWrite (motor2A, 0);
- analogWrite (motor2B, 0);
- }
- if(state == "q")
- {
- analogWrite (Gun, 255);
- }
- else if(state == "r")
- {
- analogWrite (Gun, 0);
- }
- state ="";}
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement