CUTSIE WHUN Version 2 - The Ultimate Balancing Robot

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

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(); 
}