| 1 | #include "Arduino.h" |
| 2 | #include "ard1.h" |
| 3 | #include "serial.h" |
| 4 | #include "reset.h" |
| 5 |
|
| 6 | #include "SoftwareSerial.h" |
| 7 | SoftwareSerial roboclaw(4, 5); |
| 8 |
|
| 9 | // One time |
| 10 | void setup() { |
| 11 | pinMode(PIN_RESET, INPUT); |
| 12 | digitalWrite(PIN_RESET, LOW); |
| 13 | serial(); |
| 14 |
|
| 15 | Serial.println("ARD1: ARD1 serial communications active"); |
| 16 | Serial.print("ARD1: Beginning RoboClaw Motor Controller serial at baud rate "); |
| 17 | Serial.println(ROBOCLAW_BAUD); |
| 18 | roboclaw.begin(ROBOCLAW_BAUD); |
| 19 |
|
| 20 | pinMode(PIN_BRAKES, OUTPUT); |
| 21 | pinMode(PIN_CH_1, INPUT); |
| 22 | pinMode(PIN_CH_2, INPUT); |
| 23 | pinMode(PIN_CH_3, INPUT); |
| 24 | pinMode(PIN_CH_4, INPUT); |
| 25 | pinMode(PIN_CH_5, INPUT); |
| 26 | pinMode(PIN_CH_6, INPUT); |
| 27 | } |
| 28 |
|
| 29 | int brakes = 1; |
| 30 |
|
| 31 | byte full_stop = 0; |
| 32 | // Repeatedly |
| 33 | void loop() { |
| 34 | int ch1 = pulseIn(PIN_CH_1, 1000); |
| 35 | int ch2 = pulseIn(PIN_CH_2, 1000); |
| 36 | int ch3 = pulseIn(PIN_CH_3, 1000); |
| 37 | int ch4 = pulseIn(PIN_CH_4, 1000); |
| 38 | int ch5 = pulseIn(PIN_CH_5, 1000); |
| 39 | int ch6 = pulseIn(PIN_CH_6, 1000); |
| 40 |
|
| 41 | // ch1: right joystick horizontal, 1099, 1978 |
| 42 | // ch2: right joystick vertical, 1003, 1986 |
| 43 | // ch3: left joystick vertical, 993, 1885 |
| 44 | // ch4: left joystick horizontal, 1023, 1986 |
| 45 | // ch5: switch d, 993, 1986 |
| 46 | // ch6: switch a, 993, 1986 |
| 47 |
|
| 48 | if (ch1 < 1099) ch1 = 1099; |
| 49 | if (ch1 > 1978) ch1 = 1978; |
| 50 |
|
| 51 | if (ch2 < 1003) ch2 = 1003; |
| 52 | if (ch2 > 1986) ch2 = 1986; |
| 53 |
|
| 54 | if (ch3 < 993) ch3 = 993; |
| 55 | if (ch3 > 1885) ch3 = 1885; |
| 56 |
|
| 57 | if (ch4 < 1023) ch4 = 1023; |
| 58 | if (ch4 > 1986) ch4 = 1986; |
| 59 |
|
| 60 | if (ch5 < 993) ch5 = 993; |
| 61 | if (ch5 > 1986) ch5 = 1986; |
| 62 |
|
| 63 | if (ch6 < 993) ch6 = 993; |
| 64 | if (ch6 > 1986) ch6 = 1986; |
| 65 |
|
| 66 | int ch1_fixed = map(ch1, 1099, 1978, 1, 127); |
| 67 | int ch2_fixed = map(ch2, 1003, 1986, 1, 127); |
| 68 |
|
| 69 | if (ch1_fixed < 84 && ch1_fixed > 44) ch1_fixed = 64; |
| 70 | if (ch2_fixed < 84 && ch2_fixed > 44) ch2_fixed = 64; |
| 71 |
|
| 72 | int motor_left = 64; |
| 73 | int motor_right = 64; |
| 74 |
|
| 75 | if (ch1_fixed > 64 && ch2_fixed > 64) { |
| 76 | motor_left = ch2_fixed; |
| 77 | motor_right = 127 - (ch1_fixed - 64); |
| 78 | } else if (ch1_fixed < 64 && ch2_fixed > 64) { |
| 79 | motor_right = ch2_fixed; |
| 80 | motor_left = 127 - (64 - ch1_fixed); |
| 81 | } else if (ch2_fixed > 64) { |
| 82 | motor_right = ch2_fixed; |
| 83 | motor_left = ch2_fixed; |
| 84 | } else if (ch1_fixed > 64 && ch2_fixed < 64) { |
| 85 | motor_left = ch2_fixed; |
| 86 | motor_right = (64 - (64 - ch1_fixed)) - 64; |
| 87 | } else if (ch1_fixed < 64 && ch2_fixed < 64) { |
| 88 | motor_right = ch2_fixed; |
| 89 | motor_left = (64 - (ch1_fixed + 64)) + 64; |
| 90 | } else if (ch2_fixed < 64) { |
| 91 | motor_left = ch2_fixed; |
| 92 | motor_right = ch2_fixed; |
| 93 | } else if (ch1_fixed < 64) { |
| 94 | motor_right = (64 - ch1_fixed) + 64; |
| 95 | motor_left = (64 + ch1_fixed) - 64; |
| 96 | } |
| 97 |
|
| 98 | Serial.print(motor_left); |
| 99 | Serial.print("\t"); |
| 100 | Serial.println(motor_right); |
| 101 |
|
| 102 | // roboclaw.write(127); |
| 103 | // roboclaw.write(255); |
| 104 | } |
| 105 |
|
| 106 |
|
Index