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 progress3,103
CUTSIE WHUN Version 2 - The Ultimate Balancing Robot

Things used in this project

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

19 projects • 20 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