Pigeon-Kicker
Published © MIT

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.

AdvancedWork in progress2,250
CUTSIE WHUN Version 2 - The Ultimate Balancing Robot

Things used in this project

Hardware components

Arduino Pro Mini 328 - 5V/16MHz
SparkFun Arduino Pro Mini 328 - 5V/16MHz
×1
SparkFun Triple Axis Accelerometer and Gyro Breakout - MPU-6050
SparkFun Triple Axis Accelerometer and Gyro Breakout - MPU-6050
×1
Voltage Regulator Module
Digilent Voltage Regulator Module
×1
ZX Distance and Gesture Sensor
SparkFun ZX Distance and Gesture Sensor
×1
SparkFun Motor Driver - Dual TB6612FNG (1A)
SparkFun Motor Driver - Dual TB6612FNG (1A)
×1

Software apps and online services

Arduino IDE
Arduino IDE

Hand tools and fabrication machines

Soldering iron (generic)
Soldering iron (generic)
Hot glue gun (generic)
Hot glue gun (generic)

Story

Read more

Schematics

Fritzing Breadboard

Cutsie Whun Schematic

Cutsie Whun Fritzing

Cutsie Whun v2 ino

Code

Cutsie Whun v2

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

Credits

Pigeon-Kicker

Pigeon-Kicker

3 projects • 17 followers
Computer guru from the 80's, currently disabled veteran. Building this stuffs for my son to learn robotics.
Thanks to Mini pigeon Kicker.

Comments

Add projectSign up / Login