Rocket_Payload_Arduino_V1/PayloadSystem.ino at main · UMSATS/Rocket_Payload_Arduino_V1 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79

#include <Adafruit_MotorShield.h>
#include <SoftwareSerial.h>
#include <Wire.h>

Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *myMotor1 = AFMS.getMotor(1);
Adafruit_DCMotor *myMotor2 = AFMS.getMotor(2);
Adafruit_DCMotor *myMotor3 = AFMS.getMotor(3);
Adafruit_DCMotor *myMotor4 = AFMS.getMotor(4);
SoftwareSerial Genotronex(10,11); //(Tx,Rx)

int motorSpeed = 1000;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);

  Genotronex.begin(9600);
  AFMS.begin();
  myMotor1->setSpeed(motorSpeed);
  myMotor2->setSpeed(motorSpeed);
  myMotor3->setSpeed(motorSpeed);
  myMotor4->setSpeed(motorSpeed);

}

void loop() {
  // put your main code here, to run repeatedly:
//
  if(true){
    char control = Genotronex.read();
    //Check for no button press value of control!!!
    Serial.print("Control:");
    Serial.println(control);


    if(control == 'u'){ //Move forward
      myMotor4->run(FORWARD);
      myMotor2->run(FORWARD);
    }
    else if(control == 'l'){ //Left
      myMotor4->run(BACKWARD);
      myMotor2->run(FORWARD);
    }
    else if(control == 'r'){ //Right
      myMotor4->run(FORWARD);
      myMotor2->run(BACKWARD);
    }
    else if(control == 'd'){ //Back
      myMotor4->run(BACKWARD);
      myMotor2->run(BACKWARD);
    }
    else if(control == 'x'){ //Stop rover movement
      myMotor4->run(RELEASE);
      myMotor2->run(RELEASE);
      myMotor3->run(RELEASE);
      myMotor1->run(RELEASE);
    }

    else if(control == 'i'){ //Fold rover arms in
      myMotor3->run(FORWARD);
      myMotor1->run(BACKWARD);
    }
    else if(control == 'o'){ //Fold rover arms out
      myMotor3->run(BACKWARD);
      myMotor1->run(FORWARD);
    }
    else{
    }
  }
  delay(50);
}

//    if(control == 'u'){ //Move forward
//      myMotor2->run(FORWARD);
//    }
//  }
//}