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.  

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.
  • RC to Arduino: rc_pwm-1000/4
  • Arduino to RC: arduino_pwm*4+1000

Below is a sample of the serial monitor output window.

 

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)

 

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

 

 

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