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
#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;
double ki = 0;
double kd = 0;
double Setpoint = 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);
double output = kp * error + ITerm + kd * dInput;
if (output > outMax) output = outMax;
else if (output < outMin) output = outMin;
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();
}
[SF] Pigeon Kicker -- AkA James Ryan