Arduino RC Car with Flysky FS-I6X Controller and Receiver

1/* RC CAR, Translated from Bluetooth Amazon Car to work with Arduino Mega,  Flysky FS-I6X Controller and Receiver and Adafruit Motor Shield v2.3
2
3Special  thanks to Youtube Channel "DroneBot Workshop" for providing a Template for the  Code - https://dronebotworkshop.com/radio-control-arduino-car/
4Channel functions  by Ricardo Paiva - https://gist.github.com/werneckpaiva/
5
6Written by Enrique  Lopez - reference "https://enriquelopezcode.github.io/projects/rc_car" and click  on the Arduino Projects Hub link for required parts and wiring  */
7
8//Include  Libraries
9#include <Wire.h>
10#include <Adafruit_MotorShield.h>
11#include  "utility/Adafruit_MS_PWMServoDriver.h"
12#include <IBusBM.h>
13
14//Initialize  Motors
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//  Create iBus Object and Channel Values
23IBusBM ibus;
24
25int rcCH1 = 0;  //  Acceleration
26int rcCH2 = 0;  // Left - Right
27int rcCH3 = 0;  // Forward -  Backward
28
29//Motor Speed Variables
30int motor1speed = 0;
31int motor2speed  = 0;
32int motor3speed = 0;
33int motor4speed = 0;
34
35//Motor Direction variables
36int  motor1dir = 0;
37int motor2dir = 0;
38int motor3dir = 0;
39int motor4dir = 0;
40
41//  Read the number of a given channel and convert to the range provided.
42// If the  channel is off, return the default value (by Ricardo Paiva)
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// Control Motor
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  //Call begin on Motorshield  Object and Delay for Connection
95  AFMS.begin();
96  ibus.begin(Serial1);
97  delay(2000);
98  
99  
100}
101
102void loop() {
103  //Get Channel Values
104  rcCH1 = readChannel(0, -100, 100, 0);
105  rcCH2 = readChannel(1, -100, 100, 0);
106  rcCH3 = readChannel(2, 0, 155, 0);
107
108  //Motor Speeds Channel 3
109  motor1speed  = rcCH3;
110  motor2speed = rcCH3;
111  motor3speed = rcCH3;
112  motor4speed =  rcCH3;
113
114  //Forward and Backward with Channel 2 Value
115  if (rcCH2 >= 0)  {
116    //Forward
117    motor1dir = 1;
118    motor2dir = 1;
119    motor3dir  = 1;
120    motor4dir = 1;
121  } else {
122    //Backward
123    motor1dir = -1;
124    motor2dir = -1;
125    motor3dir = -1;
126    motor4dir = -1;
127  }
128
129  //Add Channel 2 Speed
130  motor1speed = motor1speed + abs(rcCH2);
131  motor2speed  = motor2speed + abs(rcCH2);
132  motor3speed = motor3speed + abs(rcCH2);
133  motor4speed  = motor4speed + abs(rcCH2);
134
135  // Set left/right offset with channel 1 value
136  motor1speed = motor1speed + rcCH1;
137  motor2speed = motor2speed + rcCH1;
138  motor3speed = motor3speed - rcCH1;
139  motor4speed = motor4speed - rcCH1;
140
141  // Ensure that speeds are between 0 and 255
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  //Drive Motors
148
149  mControl1(motor1speed, motor1dir);
150  mControl2(motor2speed, motor2dir);
151  mControl3(motor3speed, motor3dir);
152  mControl4(motor4speed, motor4dir);
153
154  //small delay
155  delay(50);
156}