Project in progress
CUTSIE WHUN Version 2 - The Ultimate Balancing Robot

CUTSIE WHUN Version 2 - The Ultimate Balancing Robot © MIT

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.

  • 1,718 views
  • 0 comments
  • 8 respects

Components and supplies

Necessary tools and machines

09507 01
Soldering iron (generic)
Hy gluegun
Hot glue gun (generic)

Apps and online services

About this project

Overview

You can visit my Google+ Community any time...

This project uses some abnormal hardware, such as the 6 channel RC receiver module. Although you can use one with fewer channels, this project will remain using 6. Lets get this one out of the way right now, as it is the main focus of this posting for the project.

The PWM signal difference: RC receiver vs the Arduino.

  • The PWM signal for the RC receiver is sent out in a microsecond pulse, between 1000-low and 2000-high.
  • The PWM signal for the Arduino H-Bridge motor module is a power pulse, between 0-low and 255-high.
  • We are using interrupts to gather the RC receiver signals, this is a way to time the duration of the pulse and assign it a value the Arduino can work with.
  • The basic calculation between signal types is quick and easy.

Sample

Below is a sample of the serial monitor output window.

Cycle #89 
Clear distance to front:	0-In	0-Cm 
	    Temperature:	21-C	71-F 
   System Arming State:	Disarmed 
    Transmission State: 	Neutral 
	     Turn State:	Going Straight 
	     Spin State:	Not Spinning 
        Throttle State: 	0% 
 Right Stick Horizontal:	Remote CH1	1496-126 
   Right Stick Verticle:	Remote CH2	1512-130 
  Left Stick Horizontal:	Remote CH4	1444-113 
    Left Stick Verticle:	Remote CH3	1000-0 
   Arming switch signal:	Remote CH5	1992-252 
  Mode Dial(Delay time):	Remote CH6	1992-252 
Gyro Motion-Acceleration:	AX-4148		AY-448		AZ17456	 
   Gyro Motion-Rotation:	GX-1034		GY361		GZ-105 
      Gyro Acceleration:	AX-4116		AY-568		AZ17408 
          Gyro Rotation:	GX-1023		GY363		GZ-53  

Code is long, and may require more than a couple minutes for you to comprehend, skill based of course.

The functions of the RC receiver are as follows:

  • CH1 - Left/Right turn
  • CH2 - Transmission Forward/Neutral/Reverse
  • CH3 - Throttle
  • CH4 - Left/Right spin (Only usable if transmission is in neutral)
  • CH5 - Arming/Disarming switch
  • CH6 - System delay adjustment (increases/decreases the main delay in main loop)

Video

Basic systems information video

The code is 440 lines long so far.

#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:\t"); Serial.print("Remote CH1\t"); Serial.print(rc_values[RC_CH1]); Serial.print("-"); Serial.println(pwm_ch1); 
 Serial.print("    Right Stick Verticle:\t"); Serial.print("Remote CH2\t"); Serial.print(rc_values[RC_CH2]); Serial.print("-"); Serial.println(pwm_ch2); 
 Serial.print("   Left Stick Horizontal:\t"); Serial.print("Remote CH4\t"); Serial.print(rc_values[RC_CH4]); Serial.print("-"); Serial.println(pwm_ch4); 
 Serial.print("     Left Stick Verticle:\t"); Serial.print("Remote CH3\t"); Serial.print(rc_values[RC_CH3]); Serial.print("-"); Serial.println(pwm_ch3); 
 Serial.print("    Arming switch signal:\t"); Serial.print("Remote CH5\t"); Serial.print(rc_values[RC_CH5]); Serial.print("-"); Serial.println(pwm_ch5); 
 Serial.print("   Mode Dial(Delay time):\t"); Serial.print("Remote CH6\t"); 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:\t"); 
 Serial.print(sonar.ping_in()); 
 Serial.print("-In\t"); 
 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:\t"); 
 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("\t     Spin State:\t"); 
     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("\t     Turn State:\t"); 
 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: \t"); 
 throttle=map(pwm_ch3, pwm_MIN, pwm_MAX, 0, 100); 
 Serial.print(throttle);Serial.println("%"); 
} 
void transmission_state() { 
 Serial.print("     Transmission State: \t"); 
 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:\t"); 
 Serial.print("AX"); Serial.print(ax); Serial.print("\t"); 
 Serial.print("\tAY"); Serial.print(ay); Serial.print("\t"); 
 Serial.print("\tAZ"); Serial.print(az); Serial.println("\t"); 
 Serial.print("    Gyro Motion-Rotation:\t"); 
 Serial.print("GX"); Serial.print(gx); Serial.print("\t"); 
 Serial.print("\tGY"); Serial.print(gy); Serial.print("\t"); 
 Serial.print("\tGZ"); Serial.println(gz); 
 Serial.print("       Gyro Acceleration:\t"); 
 Serial.print("AX"); Serial.print(ax1); Serial.print("\t"); 
 Serial.print("\tAY"); Serial.print(ay1); Serial.print("\t"); 
 Serial.print("\tAZ"); Serial.println(az1); 
 Serial.print("           Gyro Rotation:\t"); 
 Serial.print("GX"); Serial.print(gx1); Serial.print("\t"); 
 Serial.print("\tGY"); Serial.print(gy1); Serial.print("\t"); 
 Serial.print("\tGZ"); 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("\t    Temperature:\t");Serial.print(tempC);Serial.print("-C\t");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(); 
} 

Code

Cutsie Whun v2Arduino
#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:\t"); Serial.print("Remote CH1\t"); Serial.print(rc_values[RC_CH1]); Serial.print("-"); Serial.println(pwm_ch1);
  Serial.print("    Right Stick Verticle:\t"); Serial.print("Remote CH2\t"); Serial.print(rc_values[RC_CH2]); Serial.print("-"); Serial.println(pwm_ch2);
  Serial.print("   Left Stick Horizontal:\t"); Serial.print("Remote CH4\t"); Serial.print(rc_values[RC_CH4]); Serial.print("-"); Serial.println(pwm_ch4);
  Serial.print("     Left Stick Verticle:\t"); Serial.print("Remote CH3\t"); Serial.print(rc_values[RC_CH3]); Serial.print("-"); Serial.println(pwm_ch3);
  Serial.print("    Arming switch signal:\t"); Serial.print("Remote CH5\t"); Serial.print(rc_values[RC_CH5]); Serial.print("-"); Serial.println(pwm_ch5);
  Serial.print("   Mode Dial(Delay time):\t"); Serial.print("Remote CH6\t"); 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:\t");
  Serial.print(sonar.ping_in());
  Serial.print("-In\t");
  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:\t");
  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("\t     Spin State:\t");
      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("\t     Turn State:\t");
  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: \t");
  throttle=map(pwm_ch3, pwm_MIN, pwm_MAX, 0, 100);
  Serial.print(throttle);Serial.println("%");
}

void transmission_state() {
  Serial.print("     Transmission State: \t");
  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:\t");
  Serial.print("AX"); Serial.print(ax); Serial.print("\t");
  Serial.print("\tAY"); Serial.print(ay); Serial.print("\t");
  Serial.print("\tAZ"); Serial.print(az); Serial.println("\t");
  Serial.print("    Gyro Motion-Rotation:\t");
  Serial.print("GX"); Serial.print(gx); Serial.print("\t");
  Serial.print("\tGY"); Serial.print(gy); Serial.print("\t");
  Serial.print("\tGZ"); Serial.println(gz);
  Serial.print("       Gyro Acceleration:\t");
  Serial.print("AX"); Serial.print(ax1); Serial.print("\t");
  Serial.print("\tAY"); Serial.print(ay1); Serial.print("\t");
  Serial.print("\tAZ"); Serial.println(az1);
  Serial.print("           Gyro Rotation:\t");
  Serial.print("GX"); Serial.print(gx1); Serial.print("\t");
  Serial.print("\tGY"); Serial.print(gy1); Serial.print("\t");
  Serial.print("\tGZ"); 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("\t    Temperature:\t");Serial.print(tempC);Serial.print("-C\t");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();
}

Schematics

Fritzing Breadboard
Cutsie whun bb nxqkzzvsqt
Cutsie Whun Schematic
Cutsie whun schem yrogdp7wsv
Cutsie Whun Fritzing
cutsie_whun_araVzKara4.fzz
Cutsie Whun v2 ino
cutsie_whun_v2_O2XEd3ReYv.ino

Comments

Similar projects you might like

Cutsie Whun

Project in progress by Team Pigeon Kickers Mod World

  • 816 views
  • 4 comments
  • 6 respects

Arduino Balancing Robot

Project showcase by TEAM DIY

  • 3,660 views
  • 0 comments
  • 7 respects

Otto DIY+ Arduino Bluetooth Robot Easy to 3D Print

Project tutorial by Team Otto builders

  • 48,195 views
  • 117 comments
  • 162 respects

PHPoC - Arduino Self Balancing Robot with BT+Web Control

Project in progress by Suyog Gunjal

  • 4,150 views
  • 2 comments
  • 24 respects

Linear Motion Plotter

Project in progress by Kramick Saha

  • 2,699 views
  • 2 comments
  • 8 respects

Smartphone Controlled Arduino 4WD Robot Car

Project in progress by Andriy Baranov

  • 53,263 views
  • 43 comments
  • 98 respects
Add projectSign up / Login