CUTSIE WHUN Version 2 - The Ultimate Balancing Robot

Cutsie Whun is MEGA BREAD series project #6. A tough, fast, and RC controlled 2 wheeled balancing fun machine. A lot of hard work pays off.

Sep 8, 2017

•

6507 views

•

7 respects

data collection

remote control

robots

Devices & Components

ZX Distance and Gesture Sensor

SparkFun Motor Driver - Dual TB6612FNG (1A)

Voltage Regulator Module

SparkFun Triple Axis Accelerometer and Gyro Breakout - MPU-6050

Arduino Pro Mini 328 - 5V/16MHz

Hardware & Tools

Hot glue gun (generic)

Soldering iron (generic)

Software & Tools

Arduino IDE

Project description

Code

Cutsie Whun v2

arduino

1#include <EnableInterrupt.h>
2#include <NewPing.h>
3#include "I2Cdev.h"
4#include  "MPU6050.h"
5#include "Wire.h"
6
7#define SERIAL_PORT_SPEED 57600
8#define  TRIGGER_PIN  3
9#define ECHO_PIN     4
10#define MAX_DISTANCE 500
11NewPing  sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
12#define OUTPUT_READABLE_ACCELGYRO
13MPU6050  accelgyro;
14#define heartbeat_pin 7
15int heartbeatDelay=10;
16int delayTime=0;
17int  count = 0;
18int delayMultiplier=2;
19int16_t ax, ay, az;
20int16_t gx, gy, gz;
21int16_t  ax1, ay1, az1;
22int16_t gx1, gy1, gz1;
23int x, x1;
24int y, y1;
25int z, z1;
26int  temp=0;
27int tempC=0;
28int tempF=0;
29#define RC_NUM_CHANNELS  6
30#define  RC_CH1  0
31#define RC_CH2  1
32#define RC_CH3  2
33#define RC_CH4  3
34#define  RC_CH5  4
35#define RC_CH6  5
36#define RC_CH1_INPUT  A0
37#define RC_CH2_INPUT  A1
38#define RC_CH3_INPUT  A2
39#define RC_CH4_INPUT  A3
40#define RC_CH5_INPUT  8
41#define RC_CH6_INPUT  9
42uint16_t rc_values[RC_NUM_CHANNELS];
43uint32_t  rc_start[RC_NUM_CHANNELS];
44volatile uint16_t rc_shared[RC_NUM_CHANNELS];
45
46bool  left_turn = false;
47bool right_turn = false;
48bool left_spin = false;
49bool  right_spin = false;
50bool not_spinning=true;
51bool straight = true;
52bool  reverse = false;
53bool forward = false;
54bool brakes = false;
55bool neutral  = true;
56bool armed = false;
57
58int rc_input_Deadzone = 20;
59int rc_input_MIN  = 1000;
60int rc_input_MAX = 2000;
61
62int pwm_MIN = 0;
63int pwm_MAX = 255;
64
65int  pwm_ch1 = 0;
66int pwm_ch2 = 0;
67int pwm_ch3 = 0;
68int pwm_ch4 = 0;
69int  pwm_ch5 = 0;
70int pwm_ch6 = 0;
71
72int arming_MIN = 20;
73int arming_MAX =  230;
74int forward_AT = 150;
75int reverse_AT = 90;
76int left_AT = 150;
77int  right_AT = 90;
78int throttle=0;
79
80int LF_motor_pin = 5;
81int LR_motor_pin  = 6;
82int RF_motor_pin = 10;
83int RR_motor_pin = 11;
84
85int L_motor_speed  = 0;
86int R_motor_speed = 0;
87int outMax = 255;
88int outMin = 0;
89
90float  lastInput = 0;
91double ITerm = 0;
92double kp = 2;         // Proportional value
93double  ki = 0;           // Integral value
94double kd = 0;           // Derivative value
95double  Setpoint = 0;     // Initial setpoint is 0
96double Compute(double input) {
97  double error = Setpoint - input;
98  ITerm += (ki * error);
99  if (ITerm >  outMax) ITerm = outMax;
100  else if (ITerm < outMin) ITerm = outMin;
101  double  dInput = (input - lastInput);
102  // Compute PID Output
103  double output = kp  * error + ITerm + kd * dInput;
104  if (output > outMax) output = outMax;
105  else  if (output < outMin) output = outMin;
106  // Remember some variables for next time
107  lastInput = input;
108  return output;
109}
110
111void SetSetpoint(double d)  {
112  Setpoint = d;
113}
114double GetSetPoint() {
115  return Setpoint;
116}
117
118void  rc_read_values() {
119  noInterrupts();
120  memcpy(rc_values, (const void *)rc_shared,  sizeof(rc_shared));
121  interrupts();
122}
123
124void calc_input(uint8_t channel,  uint8_t input_pin) {
125  if (digitalRead(input_pin) == HIGH) {
126    rc_start[channel]  = micros();
127  } else {
128    uint16_t rc_compare = (uint16_t)(micros() - rc_start[channel]);
129    rc_shared[channel] = rc_compare;
130  }
131}
132
133void calc_ch1() {
134  calc_input(RC_CH1,  RC_CH1_INPUT);
135  if (rc_values[RC_CH1] < rc_input_MIN) {
136    rc_values[RC_CH1]  = rc_input_MIN;
137  }
138  if (rc_values[RC_CH1] > rc_input_MAX) {
139    rc_values[RC_CH1]  = rc_input_MAX;
140  }
141}
142void calc_ch2() {
143  calc_input(RC_CH2, RC_CH2_INPUT);
144  if (rc_values[RC_CH2] < rc_input_MIN) {
145    rc_values[RC_CH2] = rc_input_MIN;
146  }
147  if (rc_values[RC_CH2] > rc_input_MAX) {
148    rc_values[RC_CH2] = rc_input_MAX;
149  }
150}
151void calc_ch3() {
152  calc_input(RC_CH3, RC_CH3_INPUT);
153  if (rc_values[RC_CH3]  < rc_input_MIN) {
154    rc_values[RC_CH3] = rc_input_MIN;
155  }
156  if (rc_values[RC_CH3]  > rc_input_MAX) {
157    rc_values[RC_CH3] = rc_input_MAX;
158  }
159}
160void calc_ch4()  {
161  calc_input(RC_CH4, RC_CH4_INPUT);
162  if (rc_values[RC_CH4] < rc_input_MIN)  {
163    rc_values[RC_CH4] = rc_input_MIN;
164  }
165  if (rc_values[RC_CH4] > rc_input_MAX)  {
166    rc_values[RC_CH4] = rc_input_MAX;
167  }
168}
169void calc_ch5() {
170  calc_input(RC_CH5, RC_CH5_INPUT);
171  if (rc_values[RC_CH5] < rc_input_MIN) {
172    rc_values[RC_CH5] = rc_input_MIN;
173  }
174  if (rc_values[RC_CH5] > rc_input_MAX)  {
175    rc_values[RC_CH5] = rc_input_MAX;
176  }
177}
178void calc_ch6() {
179  calc_input(RC_CH6, RC_CH6_INPUT);
180  if (rc_values[RC_CH6] < rc_input_MIN) {
181    rc_values[RC_CH6] = rc_input_MIN;
182  }
183  if (rc_values[RC_CH6] > rc_input_MAX)  {
184    rc_values[RC_CH6] = rc_input_MAX;
185  }
186}
187void print_rc_values()  {
188  Serial.print("  Right Stick Horizontal:\	"); Serial.print("Remote CH1\	");  Serial.print(rc_values[RC_CH1]); Serial.print("-"); Serial.println(pwm_ch1);
189  Serial.print("    Right Stick Verticle:\	"); Serial.print("Remote CH2\	");  Serial.print(rc_values[RC_CH2]); Serial.print("-"); Serial.println(pwm_ch2);
190  Serial.print("   Left Stick Horizontal:\	"); Serial.print("Remote CH4\	");  Serial.print(rc_values[RC_CH4]); Serial.print("-"); Serial.println(pwm_ch4);
191  Serial.print("     Left Stick Verticle:\	"); Serial.print("Remote CH3\	");  Serial.print(rc_values[RC_CH3]); Serial.print("-"); Serial.println(pwm_ch3);
192  Serial.print("    Arming switch signal:\	"); Serial.print("Remote CH5\	");  Serial.print(rc_values[RC_CH5]); Serial.print("-"); Serial.println(pwm_ch5);
193  Serial.print("   Mode Dial(Delay time):\	"); Serial.print("Remote CH6\	");  Serial.print(rc_values[RC_CH6]); Serial.print("-"); Serial.println(pwm_ch6);
194}
195
196void  set_rc_pwm() {
197  pwm_ch1 = map(rc_values[RC_CH1], rc_input_MIN, rc_input_MAX,  pwm_MIN, pwm_MAX);
198  if (pwm_ch1 < pwm_MIN) {    pwm_ch1 = pwm_MIN;  }
199  if  (pwm_ch1 > pwm_MAX) {    pwm_ch1 = pwm_MAX;  }
200  pwm_ch2 = map(rc_values[RC_CH2],  rc_input_MIN, rc_input_MAX, pwm_MIN, pwm_MAX);
201  if (pwm_ch2 < pwm_MIN) {    pwm_ch2  = pwm_MIN;  }
202  if (pwm_ch2 > pwm_MAX) {    pwm_ch2 = pwm_MAX;  }
203  pwm_ch3  = map(rc_values[RC_CH3], rc_input_MIN, rc_input_MAX, pwm_MIN, pwm_MAX);
204  if  (pwm_ch3 < pwm_MIN) {    pwm_ch3 = pwm_MIN;  }
205  if (pwm_ch3 > pwm_MAX) {    pwm_ch3  = pwm_MAX;  }
206  pwm_ch4 = map(rc_values[RC_CH4], rc_input_MIN, rc_input_MAX,  pwm_MIN, pwm_MAX);
207  if (pwm_ch4 < pwm_MIN) {    pwm_ch4 = pwm_MIN;  }
208  if  (pwm_ch4 > pwm_MAX) {    pwm_ch4 = pwm_MAX;  }
209  pwm_ch5 = map(rc_values[RC_CH5],  rc_input_MIN, rc_input_MAX, pwm_MIN, pwm_MAX);
210  if (pwm_ch5 < pwm_MIN) {    pwm_ch5  = pwm_MIN;  }
211  if (pwm_ch5 > pwm_MAX) {    pwm_ch5 = pwm_MAX;  }
212  pwm_ch6  = map(rc_values[RC_CH6], rc_input_MIN, rc_input_MAX, pwm_MIN, pwm_MAX);
213  if  (pwm_ch6 < pwm_MIN) {    pwm_ch6 = pwm_MIN;  }
214  if (pwm_ch6 > pwm_MAX) {    pwm_ch6  = pwm_MAX;  }
215}
216
217void Ping() {
218  Serial.print("Clear distance to front:\	");
219  Serial.print(sonar.ping_in());
220  Serial.print("-In\	");
221  Serial.print(sonar.ping_cm());
222  Serial.println("-Cm");
223}
224
225void delay_time(){
226  delayTime=pwm_ch6*delayMultiplier;
227    delay(delayTime);
228}
229
230void arming_state() {
231  Serial.print("    System  Arming State:\	");
232  if (pwm_ch5 <= arming_MIN) {
233    armed = true;
234    Serial.println("Armed");
235  } else if (pwm_ch5 >= arming_MAX) {
236    armed  = false;
237    Serial.println("Disarmed");
238  }
239}
240
241void spin_state(){
242    Serial.print("\	     Spin State:\	");
243      if (pwm_ch4 > left_AT) {
244    left_spin = true;
245    right_spin = false;
246    not_spinning = false;
247    Serial.println("Spinning Left");
248  }
249  if (pwm_ch4 < right_AT) {
250    right_spin = true;
251    left_spin = false;
252    not_spinning = false;
253    Serial.println("Spinning Right");
254  }
255  if ((pwm_ch4 < left_AT) && (pwm_ch4  > right_AT)) {
256    right_spin = false;
257    left_spin = false;
258    not_spinning  = true;
259    Serial.println("Not Spinning");
260  }
261}
262
263void turn_state()  {
264  Serial.print("\	     Turn State:\	");
265  if (pwm_ch1 > left_AT) {
266    left_turn = true;
267    right_turn = false;
268    straight = false;
269    Serial.println("Turning  Left");
270  }
271  if (pwm_ch1 < right_AT) {
272    right_turn = true;
273    left_turn  = false;
274    straight = false;
275    Serial.println("Turning Right");
276  }
277  if ((pwm_ch1 < left_AT) && (pwm_ch1 > right_AT)) {
278    right_turn =  false;
279    left_turn = false;
280    straight = true;
281    Serial.println("Going  Straight");
282  }
283}
284
285void throttle_state(){
286  Serial.print("         Throttle  State: \	");
287  throttle=map(pwm_ch3, pwm_MIN, pwm_MAX, 0, 100);
288  Serial.print(throttle);Serial.println("%");
289}
290
291void  transmission_state() {
292  Serial.print("     Transmission State: \	");
293  if (pwm_ch2 < reverse_AT) {
294    reverse = true;
295    forward = false;
296    neutral = false;
297    Serial.print("Reverse");
298  }
299  if (pwm_ch2 >  forward_AT) {
300    forward = true;
301    reverse = false;
302    neutral = false;
303    Serial.print("Forward");
304  }
305  if (pwm_ch2 < forward_AT && pwm_ch2 >  reverse_AT) {
306    reverse = false;
307    forward = false;
308    neutral = true;
309    Serial.print("Neutral");
310  }
311  Serial.println();
312}
313
314void callGyro()  {
315  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
316  accelgyro.getAcceleration(&ax1,  &ay1, &az1);
317  accelgyro.getRotation(&gx1, &gy1, &gz1);
318
319  Serial.print("Gyro  Motion-Acceleration:\	");
320  Serial.print("AX"); Serial.print(ax); Serial.print("\	");
321  Serial.print("\	AY"); Serial.print(ay); Serial.print("\	");
322  Serial.print("\	AZ");  Serial.print(az); Serial.println("\	");
323  Serial.print("    Gyro Motion-Rotation:\	");
324  Serial.print("GX"); Serial.print(gx); Serial.print("\	");
325  Serial.print("\	GY");  Serial.print(gy); Serial.print("\	");
326  Serial.print("\	GZ"); Serial.println(gz);
327  Serial.print("       Gyro Acceleration:\	");
328  Serial.print("AX"); Serial.print(ax1);  Serial.print("\	");
329  Serial.print("\	AY"); Serial.print(ay1); Serial.print("\	");
330  Serial.print("\	AZ"); Serial.println(az1);
331  Serial.print("           Gyro  Rotation:\	");
332  Serial.print("GX"); Serial.print(gx1); Serial.print("\	");
333  Serial.print("\	GY"); Serial.print(gy1); Serial.print("\	");
334  Serial.print("\	GZ");  Serial.println(gz1);
335}
336
337void get_temp() {
338temp=accelgyro.getTemperature();
339tempC=temp/340.00+36.53;
340tempF=(temp/340.00+36.53)*1.8+32;
341Serial.print("\	    Temperature:\	");Serial.print(tempC);Serial.print("-C\	");Serial.print(tempF);Serial.println("-F");
342}
343
344void  rc_motor_link() {
345  if ((reverse = true) && (armed = true)) {
346    analogWrite(LR_motor_pin,  L_motor_speed);
347    analogWrite(RR_motor_pin, R_motor_speed);
348    analogWrite(LF_motor_pin,  pwm_MIN);
349    analogWrite(RF_motor_pin, pwm_MIN);
350  }
351  if ((forward =  true) && (armed = true)) {
352    analogWrite(LF_motor_pin, L_motor_speed);
353    analogWrite(RF_motor_pin, R_motor_speed);
354    analogWrite(LR_motor_pin, pwm_MIN);
355    analogWrite(RR_motor_pin, pwm_MIN);
356  }
357  if (neutral = true) {
358    analogWrite(LR_motor_pin,  pwm_MIN);
359    analogWrite(RR_motor_pin, pwm_MIN);
360    analogWrite(LF_motor_pin,  pwm_MIN);
361    analogWrite(RF_motor_pin, pwm_MIN);
362  }
363  if (left_turn =  true) {
364    analogWrite(LR_motor_pin, pwm_MIN);
365    analogWrite(RR_motor_pin,  pwm_MIN);
366    analogWrite(LF_motor_pin, pwm_MIN);
367    analogWrite(RF_motor_pin,  R_motor_speed);
368  }
369  if (right_turn = true) {
370    analogWrite(LR_motor_pin,  pwm_MIN);
371    analogWrite(RR_motor_pin, pwm_MIN);
372    analogWrite(RF_motor_pin,  pwm_MIN);
373    analogWrite(LF_motor_pin, L_motor_speed);
374  }
375}
376void heartbeat()  {
377  digitalWrite(heartbeat_pin, HIGH);
378  delay(heartbeatDelay);
379  digitalWrite(heartbeat_pin,  LOW);
380}
381
382void setup() {
383  Serial.begin(SERIAL_PORT_SPEED);
384  pinMode(RC_CH1_INPUT,  INPUT);
385  pinMode(RC_CH2_INPUT, INPUT);
386  pinMode(RC_CH3_INPUT, INPUT);
387  pinMode(RC_CH4_INPUT, INPUT);
388  pinMode(RC_CH5_INPUT, INPUT);
389  pinMode(RC_CH6_INPUT,  INPUT);
390  pinMode(heartbeat_pin, OUTPUT);
391  enableInterrupt(RC_CH1_INPUT,  calc_ch1, CHANGE);
392  enableInterrupt(RC_CH2_INPUT, calc_ch2, CHANGE);
393  enableInterrupt(RC_CH3_INPUT,  calc_ch3, CHANGE);
394  enableInterrupt(RC_CH4_INPUT, calc_ch4, CHANGE);
395  enableInterrupt(RC_CH5_INPUT,  calc_ch5, CHANGE);
396  enableInterrupt(RC_CH6_INPUT, calc_ch6, CHANGE);
397#if  I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
398  Wire.begin();
399  Serial.println("Using  Arduino Wire");
400#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
401  Fastwire::setup(400,  true);
402  Serial.println("Using Arduino FastWire");
403#endif
404  Serial.println("Initializing  I2C devices...");
405  accelgyro.initialize();
406  Serial.println("Testing device  connections...");
407  Serial.println(accelgyro.testConnection() ? "MPU6050 connection  successful" : "MPU6050 connection failed");
408  pinMode(LF_motor_pin, OUTPUT);
409  pinMode(LR_motor_pin, OUTPUT);
410  pinMode(RF_motor_pin, OUTPUT);
411  pinMode(RR_motor_pin,  OUTPUT);
412  digitalWrite(LF_motor_pin, LOW);
413  digitalWrite(LF_motor_pin, LOW);
414  digitalWrite(LF_motor_pin, LOW);
415  digitalWrite(LF_motor_pin, LOW);
416}
417
418void  loop() {
419  heartbeat();
420  Serial.println();
421  Serial.print("Cycle #");
422  Serial.print(count);
423  Serial.println();
424  Ping();
425  get_temp();
426  arming_state();
427  transmission_state();
428  turn_state();
429  spin_state();
430  throttle_state();
431  Serial.println();
432  rc_read_values();
433  set_rc_pwm();
434  rc_motor_link();
435  print_rc_values();
436  Serial.println();
437  callGyro();
438  count++;
439  delay_time();
440}
441

#include <EnableInterrupt.h>
#include <NewPing.h>
#include "I2Cdev.h"
#include  "MPU6050.h"
#include "Wire.h"

#define SERIAL_PORT_SPEED 57600
#define  TRIGGER_PIN  3
#define ECHO_PIN     4
#define MAX_DISTANCE 500
NewPing  sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
#define OUTPUT_READABLE_ACCELGYRO
MPU6050  accelgyro;
#define heartbeat_pin 7
int heartbeatDelay=10;
int delayTime=0;
int  count = 0;
int delayMultiplier=2;
int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t  ax1, ay1, az1;
int16_t gx1, gy1, gz1;
int x, x1;
int y, y1;
int z, z1;
int  temp=0;
int tempC=0;
int tempF=0;
#define RC_NUM_CHANNELS  6
#define  RC_CH1  0
#define RC_CH2  1
#define RC_CH3  2
#define RC_CH4  3
#define  RC_CH5  4
#define RC_CH6  5
#define RC_CH1_INPUT  A0
#define RC_CH2_INPUT  A1
#define RC_CH3_INPUT  A2
#define RC_CH4_INPUT  A3
#define RC_CH5_INPUT  8
#define RC_CH6_INPUT  9
uint16_t rc_values[RC_NUM_CHANNELS];
uint32_t  rc_start[RC_NUM_CHANNELS];
volatile uint16_t rc_shared[RC_NUM_CHANNELS];

bool  left_turn = false;
bool right_turn = false;
bool left_spin = false;
bool  right_spin = false;
bool not_spinning=true;
bool straight = true;
bool  reverse = false;
bool forward = false;
bool brakes = false;
bool neutral  = true;
bool armed = false;

int rc_input_Deadzone = 20;
int rc_input_MIN  = 1000;
int rc_input_MAX = 2000;

int pwm_MIN = 0;
int pwm_MAX = 255;

int  pwm_ch1 = 0;
int pwm_ch2 = 0;
int pwm_ch3 = 0;
int pwm_ch4 = 0;
int  pwm_ch5 = 0;
int pwm_ch6 = 0;

int arming_MIN = 20;
int arming_MAX =  230;
int forward_AT = 150;
int reverse_AT = 90;
int left_AT = 150;
int  right_AT = 90;
int throttle=0;

int LF_motor_pin = 5;
int LR_motor_pin  = 6;
int RF_motor_pin = 10;
int RR_motor_pin = 11;

int L_motor_speed  = 0;
int R_motor_speed = 0;
int outMax = 255;
int outMin = 0;

float  lastInput = 0;
double ITerm = 0;
double kp = 2;         // Proportional value
double  ki = 0;           // Integral value
double kd = 0;           // Derivative value
double  Setpoint = 0;     // Initial setpoint is 0
double Compute(double input) {
  double error = Setpoint - input;
  ITerm += (ki * error);
  if (ITerm >  outMax) ITerm = outMax;
  else if (ITerm < outMin) ITerm = outMin;
  double  dInput = (input - lastInput);
  // Compute PID Output
  double output = kp  * error + ITerm + kd * dInput;
  if (output > outMax) output = outMax;
  else  if (output < outMin) output = outMin;
  // Remember some variables for next time
  lastInput = input;
  return output;
}

void SetSetpoint(double d)  {
  Setpoint = d;
}
double GetSetPoint() {
  return Setpoint;
}

void  rc_read_values() {
  noInterrupts();
  memcpy(rc_values, (const void *)rc_shared,  sizeof(rc_shared));
  interrupts();
}

void calc_input(uint8_t channel,  uint8_t input_pin) {
  if (digitalRead(input_pin) == HIGH) {
    rc_start[channel]  = micros();
  } else {
    uint16_t rc_compare = (uint16_t)(micros() - rc_start[channel]);
    rc_shared[channel] = rc_compare;
  }
}

void calc_ch1() {
  calc_input(RC_CH1,  RC_CH1_INPUT);
  if (rc_values[RC_CH1] < rc_input_MIN) {
    rc_values[RC_CH1]  = rc_input_MIN;
  }
  if (rc_values[RC_CH1] > rc_input_MAX) {
    rc_values[RC_CH1]  = rc_input_MAX;
  }
}
void calc_ch2() {
  calc_input(RC_CH2, RC_CH2_INPUT);
  if (rc_values[RC_CH2] < rc_input_MIN) {
    rc_values[RC_CH2] = rc_input_MIN;
  }
  if (rc_values[RC_CH2] > rc_input_MAX) {
    rc_values[RC_CH2] = rc_input_MAX;
  }
}
void calc_ch3() {
  calc_input(RC_CH3, RC_CH3_INPUT);
  if (rc_values[RC_CH3]  < rc_input_MIN) {
    rc_values[RC_CH3] = rc_input_MIN;
  }
  if (rc_values[RC_CH3]  > rc_input_MAX) {
    rc_values[RC_CH3] = rc_input_MAX;
  }
}
void calc_ch4()  {
  calc_input(RC_CH4, RC_CH4_INPUT);
  if (rc_values[RC_CH4] < rc_input_MIN)  {
    rc_values[RC_CH4] = rc_input_MIN;
  }
  if (rc_values[RC_CH4] > rc_input_MAX)  {
    rc_values[RC_CH4] = rc_input_MAX;
  }
}
void calc_ch5() {
  calc_input(RC_CH5, RC_CH5_INPUT);
  if (rc_values[RC_CH5] < rc_input_MIN) {
    rc_values[RC_CH5] = rc_input_MIN;
  }
  if (rc_values[RC_CH5] > rc_input_MAX)  {
    rc_values[RC_CH5] = rc_input_MAX;
  }
}
void calc_ch6() {
  calc_input(RC_CH6, RC_CH6_INPUT);
  if (rc_values[RC_CH6] < rc_input_MIN) {
    rc_values[RC_CH6] = rc_input_MIN;
  }
  if (rc_values[RC_CH6] > rc_input_MAX)  {
    rc_values[RC_CH6] = rc_input_MAX;
  }
}
void print_rc_values()  {
  Serial.print("  Right Stick Horizontal:\	"); Serial.print("Remote CH1\	");  Serial.print(rc_values[RC_CH1]); Serial.print("-"); Serial.println(pwm_ch1);
  Serial.print("    Right Stick Verticle:\	"); Serial.print("Remote CH2\	");  Serial.print(rc_values[RC_CH2]); Serial.print("-"); Serial.println(pwm_ch2);
  Serial.print("   Left Stick Horizontal:\	"); Serial.print("Remote CH4\	");  Serial.print(rc_values[RC_CH4]); Serial.print("-"); Serial.println(pwm_ch4);
  Serial.print("     Left Stick Verticle:\	"); Serial.print("Remote CH3\	");  Serial.print(rc_values[RC_CH3]); Serial.print("-"); Serial.println(pwm_ch3);
  Serial.print("    Arming switch signal:\	"); Serial.print("Remote CH5\	");  Serial.print(rc_values[RC_CH5]); Serial.print("-"); Serial.println(pwm_ch5);
  Serial.print("   Mode Dial(Delay time):\	"); Serial.print("Remote CH6\	");  Serial.print(rc_values[RC_CH6]); Serial.print("-"); Serial.println(pwm_ch6);
}

void  set_rc_pwm() {
  pwm_ch1 = map(rc_values[RC_CH1], rc_input_MIN, rc_input_MAX,  pwm_MIN, pwm_MAX);
  if (pwm_ch1 < pwm_MIN) {    pwm_ch1 = pwm_MIN;  }
  if  (pwm_ch1 > pwm_MAX) {    pwm_ch1 = pwm_MAX;  }
  pwm_ch2 = map(rc_values[RC_CH2],  rc_input_MIN, rc_input_MAX, pwm_MIN, pwm_MAX);
  if (pwm_ch2 < pwm_MIN) {    pwm_ch2  = pwm_MIN;  }
  if (pwm_ch2 > pwm_MAX) {    pwm_ch2 = pwm_MAX;  }
  pwm_ch3  = map(rc_values[RC_CH3], rc_input_MIN, rc_input_MAX, pwm_MIN, pwm_MAX);
  if  (pwm_ch3 < pwm_MIN) {    pwm_ch3 = pwm_MIN;  }
  if (pwm_ch3 > pwm_MAX) {    pwm_ch3  = pwm_MAX;  }
  pwm_ch4 = map(rc_values[RC_CH4], rc_input_MIN, rc_input_MAX,  pwm_MIN, pwm_MAX);
  if (pwm_ch4 < pwm_MIN) {    pwm_ch4 = pwm_MIN;  }
  if  (pwm_ch4 > pwm_MAX) {    pwm_ch4 = pwm_MAX;  }
  pwm_ch5 = map(rc_values[RC_CH5],  rc_input_MIN, rc_input_MAX, pwm_MIN, pwm_MAX);
  if (pwm_ch5 < pwm_MIN) {    pwm_ch5  = pwm_MIN;  }
  if (pwm_ch5 > pwm_MAX) {    pwm_ch5 = pwm_MAX;  }
  pwm_ch6  = map(rc_values[RC_CH6], rc_input_MIN, rc_input_MAX, pwm_MIN, pwm_MAX);
  if  (pwm_ch6 < pwm_MIN) {    pwm_ch6 = pwm_MIN;  }
  if (pwm_ch6 > pwm_MAX) {    pwm_ch6  = pwm_MAX;  }
}

void Ping() {
  Serial.print("Clear distance to front:\	");
  Serial.print(sonar.ping_in());
  Serial.print("-In\	");
  Serial.print(sonar.ping_cm());
  Serial.println("-Cm");
}

void delay_time(){
  delayTime=pwm_ch6*delayMultiplier;
    delay(delayTime);
}

void arming_state() {
  Serial.print("    System  Arming State:\	");
  if (pwm_ch5 <= arming_MIN) {
    armed = true;
    Serial.println("Armed");
  } else if (pwm_ch5 >= arming_MAX) {
    armed  = false;
    Serial.println("Disarmed");
  }
}

void spin_state(){
    Serial.print("\	     Spin State:\	");
      if (pwm_ch4 > left_AT) {
    left_spin = true;
    right_spin = false;
    not_spinning = false;
    Serial.println("Spinning Left");
  }
  if (pwm_ch4 < right_AT) {
    right_spin = true;
    left_spin = false;
    not_spinning = false;
    Serial.println("Spinning Right");
  }
  if ((pwm_ch4 < left_AT) && (pwm_ch4  > right_AT)) {
    right_spin = false;
    left_spin = false;
    not_spinning  = true;
    Serial.println("Not Spinning");
  }
}

void turn_state()  {
  Serial.print("\	     Turn State:\	");
  if (pwm_ch1 > left_AT) {
    left_turn = true;
    right_turn = false;
    straight = false;
    Serial.println("Turning  Left");
  }
  if (pwm_ch1 < right_AT) {
    right_turn = true;
    left_turn  = false;
    straight = false;
    Serial.println("Turning Right");
  }
  if ((pwm_ch1 < left_AT) && (pwm_ch1 > right_AT)) {
    right_turn =  false;
    left_turn = false;
    straight = true;
    Serial.println("Going  Straight");
  }
}

void throttle_state(){
  Serial.print("         Throttle  State: \	");
  throttle=map(pwm_ch3, pwm_MIN, pwm_MAX, 0, 100);
  Serial.print(throttle);Serial.println("%");
}

void  transmission_state() {
  Serial.print("     Transmission State: \	");
  if (pwm_ch2 < reverse_AT) {
    reverse = true;
    forward = false;
    neutral = false;
    Serial.print("Reverse");
  }
  if (pwm_ch2 >  forward_AT) {
    forward = true;
    reverse = false;
    neutral = false;
    Serial.print("Forward");
  }
  if (pwm_ch2 < forward_AT && pwm_ch2 >  reverse_AT) {
    reverse = false;
    forward = false;
    neutral = true;
    Serial.print("Neutral");
  }
  Serial.println();
}

void callGyro()  {
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  accelgyro.getAcceleration(&ax1,  &ay1, &az1);
  accelgyro.getRotation(&gx1, &gy1, &gz1);

  Serial.print("Gyro  Motion-Acceleration:\	");
  Serial.print("AX"); Serial.print(ax); Serial.print("\	");
  Serial.print("\	AY"); Serial.print(ay); Serial.print("\	");
  Serial.print("\	AZ");  Serial.print(az); Serial.println("\	");
  Serial.print("    Gyro Motion-Rotation:\	");
  Serial.print("GX"); Serial.print(gx); Serial.print("\	");
  Serial.print("\	GY");  Serial.print(gy); Serial.print("\	");
  Serial.print("\	GZ"); Serial.println(gz);
  Serial.print("       Gyro Acceleration:\	");
  Serial.print("AX"); Serial.print(ax1);  Serial.print("\	");
  Serial.print("\	AY"); Serial.print(ay1); Serial.print("\	");
  Serial.print("\	AZ"); Serial.println(az1);
  Serial.print("           Gyro  Rotation:\	");
  Serial.print("GX"); Serial.print(gx1); Serial.print("\	");
  Serial.print("\	GY"); Serial.print(gy1); Serial.print("\	");
  Serial.print("\	GZ");  Serial.println(gz1);
}

void get_temp() {
temp=accelgyro.getTemperature();
tempC=temp/340.00+36.53;
tempF=(temp/340.00+36.53)*1.8+32;
Serial.print("\	    Temperature:\	");Serial.print(tempC);Serial.print("-C\	");Serial.print(tempF);Serial.println("-F");
}

void  rc_motor_link() {
  if ((reverse = true) && (armed = true)) {
    analogWrite(LR_motor_pin,  L_motor_speed);
    analogWrite(RR_motor_pin, R_motor_speed);
    analogWrite(LF_motor_pin,  pwm_MIN);
    analogWrite(RF_motor_pin, pwm_MIN);
  }
  if ((forward =  true) && (armed = true)) {
    analogWrite(LF_motor_pin, L_motor_speed);
    analogWrite(RF_motor_pin, R_motor_speed);
    analogWrite(LR_motor_pin, pwm_MIN);
    analogWrite(RR_motor_pin, pwm_MIN);
  }
  if (neutral = true) {
    analogWrite(LR_motor_pin,  pwm_MIN);
    analogWrite(RR_motor_pin, pwm_MIN);
    analogWrite(LF_motor_pin,  pwm_MIN);
    analogWrite(RF_motor_pin, pwm_MIN);
  }
  if (left_turn =  true) {
    analogWrite(LR_motor_pin, pwm_MIN);
    analogWrite(RR_motor_pin,  pwm_MIN);
    analogWrite(LF_motor_pin, pwm_MIN);
    analogWrite(RF_motor_pin,  R_motor_speed);
  }
  if (right_turn = true) {
    analogWrite(LR_motor_pin,  pwm_MIN);
    analogWrite(RR_motor_pin, pwm_MIN);
    analogWrite(RF_motor_pin,  pwm_MIN);
    analogWrite(LF_motor_pin, L_motor_speed);
  }
}
void heartbeat()  {
  digitalWrite(heartbeat_pin, HIGH);
  delay(heartbeatDelay);
  digitalWrite(heartbeat_pin,  LOW);
}

void setup() {
  Serial.begin(SERIAL_PORT_SPEED);
  pinMode(RC_CH1_INPUT,  INPUT);
  pinMode(RC_CH2_INPUT, INPUT);
  pinMode(RC_CH3_INPUT, INPUT);
  pinMode(RC_CH4_INPUT, INPUT);
  pinMode(RC_CH5_INPUT, INPUT);
  pinMode(RC_CH6_INPUT,  INPUT);
  pinMode(heartbeat_pin, OUTPUT);
  enableInterrupt(RC_CH1_INPUT,  calc_ch1, CHANGE);
  enableInterrupt(RC_CH2_INPUT, calc_ch2, CHANGE);
  enableInterrupt(RC_CH3_INPUT,  calc_ch3, CHANGE);
  enableInterrupt(RC_CH4_INPUT, calc_ch4, CHANGE);
  enableInterrupt(RC_CH5_INPUT,  calc_ch5, CHANGE);
  enableInterrupt(RC_CH6_INPUT, calc_ch6, CHANGE);
#if  I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  Serial.println("Using  Arduino Wire");
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400,  true);
  Serial.println("Using Arduino FastWire");
#endif
  Serial.println("Initializing  I2C devices...");
  accelgyro.initialize();
  Serial.println("Testing device  connections...");
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection  successful" : "MPU6050 connection failed");
  pinMode(LF_motor_pin, OUTPUT);
  pinMode(LR_motor_pin, OUTPUT);
  pinMode(RF_motor_pin, OUTPUT);
  pinMode(RR_motor_pin,  OUTPUT);
  digitalWrite(LF_motor_pin, LOW);
  digitalWrite(LF_motor_pin, LOW);
  digitalWrite(LF_motor_pin, LOW);
  digitalWrite(LF_motor_pin, LOW);
}

void  loop() {
  heartbeat();
  Serial.println();
  Serial.print("Cycle #");
  Serial.print(count);
  Serial.println();
  Ping();
  get_temp();
  arming_state();
  transmission_state();
  turn_state();
  spin_state();
  throttle_state();
  Serial.println();
  rc_read_values();
  set_rc_pwm();
  rc_motor_link();
  print_rc_values();
  Serial.println();
  callGyro();
  count++;
  delay_time();
}

Downloadable files

Cutsie Whun v2 ino

Fritzing Breadboard

Cutsie Whun Fritzing

Cutsie Whun Schematic

Cutsie Whun v2 ino

Cutsie Whun Schematic

Fritzing Breadboard

Cutsie Whun Fritzing

Comments

Only logged in users can leave comments

Pigeon-Kicker

0 Followers

•

0 Projects

Work attribution

Intro

Trademarks & CopyrightsWhistleblowingDigital Services ActTerms of ServicePrivacy PolicySecurityCookie Settings

Report content