1
7
8
9#include <Wire.h>
10#include <Adafruit_MotorShield.h>
11#include "utility/Adafruit_MS_PWMServoDriver.h"
12#include <IBusBM.h>
13
14
15Adafruit_MotorShield AFMS = Adafruit_MotorShield();
16Adafruit_DCMotor *motor1 = AFMS.getMotor(1);
17Adafruit_DCMotor *motor2 = AFMS.getMotor(2);
18Adafruit_DCMotor *motor3 = AFMS.getMotor(3);
19Adafruit_DCMotor *motor4 = AFMS.getMotor(4);
20
21
22
23IBusBM ibus;
24
25int rcCH1 = 0;
26int rcCH2 = 0;
27int rcCH3 = 0;
28
29
30int motor1speed = 0;
31int motor2speed = 0;
32int motor3speed = 0;
33int motor4speed = 0;
34
35
36int motor1dir = 0;
37int motor2dir = 0;
38int motor3dir = 0;
39int motor4dir = 0;
40
41
42
43int readChannel(byte channelInput, int minLimit, int maxLimit, int defaultValue) {
44 uint16_t ch = ibus.readChannel(channelInput);
45 if (ch < 100) return defaultValue;
46 return map(ch, 1000, 2000, minLimit, maxLimit);
47}
48
49
50
51void mControl1(int mspeed, int mdir) {
52 motor1->setSpeed(mspeed);
53 if (mdir == 1) {
54 motor1->run(FORWARD);
55 } else {
56 motor1->run(BACKWARD);
57 }
58}
59
60void mControl2(int mspeed, int mdir) {
61 motor2->setSpeed(mspeed);
62 if (mdir == 1) {
63 motor2->run(FORWARD);
64 } else {
65 motor2->run(BACKWARD);
66 }
67}
68
69void mControl3(int mspeed, int mdir) {
70 motor3->setSpeed(mspeed);
71 if (mdir == 1) {
72 motor3->run(FORWARD);
73 } else {
74 motor3->run(BACKWARD);
75 }
76}
77
78void mControl4(int mspeed, int mdir) {
79 motor4->setSpeed(mspeed);
80 if (mdir == 1) {
81 motor4->run(FORWARD);
82 } else {
83 motor4->run(BACKWARD);
84 }
85}
86
87
88
89
90
91
92
93void setup() {
94
95 AFMS.begin();
96 ibus.begin(Serial1);
97 delay(2000);
98
99
100}
101
102void loop() {
103
104 rcCH1 = readChannel(0, -100, 100, 0);
105 rcCH2 = readChannel(1, -100, 100, 0);
106 rcCH3 = readChannel(2, 0, 155, 0);
107
108
109 motor1speed = rcCH3;
110 motor2speed = rcCH3;
111 motor3speed = rcCH3;
112 motor4speed = rcCH3;
113
114
115 if (rcCH2 >= 0) {
116
117 motor1dir = 1;
118 motor2dir = 1;
119 motor3dir = 1;
120 motor4dir = 1;
121 } else {
122
123 motor1dir = -1;
124 motor2dir = -1;
125 motor3dir = -1;
126 motor4dir = -1;
127 }
128
129
130 motor1speed = motor1speed + abs(rcCH2);
131 motor2speed = motor2speed + abs(rcCH2);
132 motor3speed = motor3speed + abs(rcCH2);
133 motor4speed = motor4speed + abs(rcCH2);
134
135
136 motor1speed = motor1speed + rcCH1;
137 motor2speed = motor2speed + rcCH1;
138 motor3speed = motor3speed - rcCH1;
139 motor4speed = motor4speed - rcCH1;
140
141
142 motor1speed = constrain(motor1speed, 0, 255);
143 motor2speed = constrain(motor2speed, 0, 255);
144 motor3speed = constrain(motor3speed, 0, 255);
145 motor4speed = constrain(motor4speed, 0, 255);
146
147
148
149 mControl1(motor1speed, motor1dir);
150 mControl2(motor2speed, motor2dir);
151 mControl3(motor3speed, motor3dir);
152 mControl4(motor4speed, motor4dir);
153
154
155 delay(50);
156}