This is a durability test. It is only a test so do not be alarmed at the sound of grinding gears, any sight of missing pieces or the inability to understand what this phase in Cutsie Whun's life is all about.
(Giggle snort) I want to break you Cutsie Whun.
All other systems are functional, except the gyro activation. All the code works, except the gyro stuff. Just so you can recognize him after the mayhem, here is a before the mayhem picture.
A before the mayhem picture
And now onto the attempt at robotic destruction. LOLZ, enjoy!
#include <EnableInterrupt.h> //RC receiver and Gyro interrupts
#include <NewPing.h> // Sonic distance
#include "I2Cdev.h" // Gyro comms
#include "Wire.h" // Gyro comms
#include "MPU6050_6Axis_MotionApps20.h" //DMP apps
#define SERIAL_PORT_SPEED 115200
#define TRIGGER_PIN 3
#define ECHO_PIN 4
#define MAX_DISTANCE 400
#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
#define heartbeat_pin 7
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
uint16_t rc_values[RC_NUM_CHANNELS];
uint32_t rc_start[RC_NUM_CHANNELS];
volatile uint16_t rc_shared[RC_NUM_CHANNELS];
int rc_input_Deadzone = 20;
int rc_input_min = 1000;
int rc_input_max = 2000;
int pwm_out_min = 0;
int pwm_out_max = 255;
int pwm_out_rev = -255;
int pwm_ch1, pwm_ch2, pwm_ch3, pwm_ch4, pwm_ch5, pwm_ch6, throttle_pwm;
int LF_motor_pin = 10;
int LR_motor_pin = 6;
int RF_motor_pin = 5;
int RR_motor_pin = 11;
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 L_motor_adjustment = 18, R_motor_adjustment = 0;
int heartbeatDelay = 5, delay_time = 255, delayMultiplier = 3, count;
int temp, tempC, tempF;
volatile bool armed = false;
volatile bool left_turn = false;
volatile bool right_turn = false;
volatile bool no_turn = true;
volatile bool left_spin = false;
volatile bool right_spin = false;
volatile bool no_spin = true;
volatile bool reverse = false;
volatile bool forward = false;
volatile bool neutral = true;
int L_gyro_speed, R_gyro_speed, L_motor_speed, R_motor_speed;
int gyro_counter, gyro_input, gyro_output, gyro_counter_max = 50, input_last;
double gyro_output_max, gyro_output_min;
double input, output, ax, ay, az, gx, gy, gz, ITerm;
double kp = 5;
double ki = 2;
double kd = 3;
double Setpoint = 110;
double XAccelOffset, YAccelOffset, ZAccelOffset;
double XGyroOffset, YGyroOffset, ZGyroOffset;
MPU6050 internal_mpu;//MPU6050 internal_gyro;
volatile bool dmpReady = true;
volatile bool mpuInterrupt = true;
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
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;
}
}
/*MEGA BREAD SERIES #7
The CUTSIE WHUN Project
DMP Voids
Updated 9-18-2017
Google+ Community Page Link: https://plus.google.com/communities/108091771016945233997
*/
void dmpDataReady() {
mpuInterrupt = true;
}
void dmp_loop() {
if (!dmpReady) return;
while (!mpuInterrupt && fifoCount < packetSize) {}
mpuInterrupt = false;
mpuIntStatus = internal_mpu.getIntStatus();
fifoCount = internal_mpu.getFIFOCount();
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
internal_mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
} else if (mpuIntStatus & 0x02) {
while (fifoCount < packetSize) fifoCount = internal_mpu.getFIFOCount();
internal_mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
internal_mpu.dmpGetQuaternion(&q, fifoBuffer); internal_mpu.dmpGetGravity(&gravity, &q); internal_mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial.print("ypr\t"); Serial.print(ypr[0] * 180 / M_PI); Serial.print("\t"); Serial.print(ypr[1] * 180 / M_PI); Serial.print("\t"); Serial.println(ypr[2] * 180 / M_PI);
internal_mpu.dmpGetQuaternion(&q, fifoBuffer); internal_mpu.dmpGetAccel(&aa, fifoBuffer);
internal_mpu.dmpGetGravity(&gravity, &q); internal_mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
Serial.print("areal\t"); Serial.print(aaReal.x); Serial.print("\t"); Serial.print(aaReal.y); Serial.print("\t"); Serial.println(aaReal.z);
}
}
/*MEGA BREAD SERIES #7
The CUTSIE WHUN Project
Motor Voids
Updated 9-18-2017
Google+ Community Page Link: https://plus.google.com/communities/108091771016945233997
*/
void gyro_motor_link() {
if (output > 0) {
analogWrite(LR_motor_pin, output); analogWrite(RR_motor_pin, output);
analogWrite(LF_motor_pin, pwm_out_min); analogWrite(RF_motor_pin, pwm_out_min);
}
if (output < 0) {
analogWrite(LF_motor_pin, output); analogWrite(RF_motor_pin, output);
analogWrite(LR_motor_pin, pwm_out_min); analogWrite(RR_motor_pin, pwm_out_min);
}
Serial.print("Input and Output:\t"); Serial.print("Input"); Serial.print(input); Serial.print("\t");
Serial.print("\tOutput"); Serial.println(output);
Serial.print("Gyro Motion-Acceleration:\t"); Serial.print("AX"); Serial.println(ax);
Serial.print(" Gyro Motion-Rotation:\t"); Serial.print("GX"); Serial.println(gx);
}
void rc_motor_link() {
L_motor_speed = pwm_ch3; R_motor_speed = pwm_ch3;
if (no_spin == true) {
analogWrite(LR_motor_pin, pwm_out_min); analogWrite(RR_motor_pin, pwm_out_min);
analogWrite(LF_motor_pin, pwm_out_min); analogWrite(RF_motor_pin, pwm_out_min);
}
if (no_turn == true) {
analogWrite(LR_motor_pin, pwm_out_min); analogWrite(RR_motor_pin, pwm_out_min);
analogWrite(LF_motor_pin, pwm_out_min); analogWrite(RF_motor_pin, pwm_out_min);
}
if (neutral == true) {
analogWrite(LR_motor_pin, pwm_out_min); analogWrite(RR_motor_pin, pwm_out_min);
analogWrite(LF_motor_pin, pwm_out_min); analogWrite(RF_motor_pin, pwm_out_min);
}
if (reverse == true) {
analogWrite(LR_motor_pin, L_motor_speed); analogWrite(RR_motor_pin, R_motor_speed);
analogWrite(LF_motor_pin, pwm_out_min); analogWrite(RF_motor_pin, pwm_out_min);
}
if (forward == true) {
analogWrite(LF_motor_pin, L_motor_speed); analogWrite(RF_motor_pin, R_motor_speed);
analogWrite(LR_motor_pin, pwm_out_min); analogWrite(RR_motor_pin, pwm_out_min);
}
if (left_turn == true) {
analogWrite(LR_motor_pin, pwm_out_min); analogWrite(RR_motor_pin, pwm_out_min);
analogWrite(LF_motor_pin, pwm_out_min); analogWrite(RF_motor_pin, R_motor_speed);
}
if (right_turn == true) {
analogWrite(LR_motor_pin, pwm_out_min); analogWrite(RR_motor_pin, pwm_out_min);
analogWrite(RF_motor_pin, pwm_out_min); analogWrite(LF_motor_pin, L_motor_speed);
}
if (right_spin == true) {
analogWrite(LF_motor_pin, L_motor_speed); analogWrite(RR_motor_pin, R_motor_speed);
analogWrite(LR_motor_pin, pwm_out_min); analogWrite(RF_motor_pin, pwm_out_min);
}
if (left_spin == true) {
analogWrite(LR_motor_pin, L_motor_speed); analogWrite(RF_motor_pin, R_motor_speed);
analogWrite(LF_motor_pin, pwm_out_min); analogWrite(RR_motor_pin, pwm_out_min);
}
}
/*MEGA BREAD SERIES #7
The CUTSIE WHUN Project
RC Voids
Updated 9-18-2017
Google+ Community Page Link: https://plus.google.com/communities/108091771016945233997
*/
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_out_min, pwm_out_max);
if (pwm_ch1 < pwm_out_min) {
pwm_ch1 = pwm_out_min;
}
if (pwm_ch1 > pwm_out_max) {
pwm_ch1 = pwm_out_max;
}
pwm_ch2 = map(rc_values[RC_CH2], rc_input_min, rc_input_max, pwm_out_min, pwm_out_max);
if (pwm_ch2 < pwm_out_min) {
pwm_ch2 = pwm_out_min;
}
if (pwm_ch2 > pwm_out_max) {
pwm_ch2 = pwm_out_max;
}
pwm_ch3 = map(rc_values[RC_CH3], rc_input_min, rc_input_max, pwm_out_min, pwm_out_max);
if (pwm_ch3 < pwm_out_min) {
pwm_ch3 = pwm_out_min;
}
if (pwm_ch3 > pwm_out_max) {
pwm_ch3 = pwm_out_max;
}
pwm_ch4 = map(rc_values[RC_CH4], rc_input_min, rc_input_max, pwm_out_min, pwm_out_max);
if (pwm_ch4 < pwm_out_min) {
pwm_ch4 = pwm_out_min;
}
if (pwm_ch4 > pwm_out_max) {
pwm_ch4 = pwm_out_max;
}
pwm_ch5 = map(rc_values[RC_CH5], rc_input_min, rc_input_max, pwm_out_min, pwm_out_max);
if (pwm_ch5 < pwm_out_min) {
pwm_ch5 = pwm_out_min;
}
if (pwm_ch5 > pwm_out_max) {
pwm_ch5 = pwm_out_max;
}
pwm_ch6 = map(rc_values[RC_CH6], rc_input_min, rc_input_max, pwm_out_min, pwm_out_max);
if (pwm_ch6 < pwm_out_min) {
pwm_ch6 = pwm_out_min;
}
if (pwm_ch6 > pwm_out_max) {
pwm_ch6 = pwm_out_max;
};
}
/*MEGA BREAD SERIES #7
The CUTSIE WHUN Project
True and False State Voids
Updated 9-18-2017
Google+ Community Page Link: https://plus.google.com/communities/108091771016945233997
*/
void throttle_state() {
Serial.print(" Throttle State: \t");
throttle_pwm = pwm_ch3;
Serial.println(throttle_pwm);
}
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 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 turn_state() {
Serial.print("\t Turn State:\t");
if (pwm_ch1 > left_AT) {
left_turn = true; right_turn = false; no_turn = false;
Serial.println("Turning Left");
}
if (pwm_ch1 < right_AT) {
right_turn = true; left_turn = false; no_turn = false;
Serial.println("Turning Right");
}
if (pwm_ch1 < left_AT && pwm_ch1 > right_AT) {
right_turn = false; left_turn = false; no_turn = true;
Serial.println("Not Turning");
} Serial.println();
}
void spin_state() {
Serial.print("\t Spin State:\t");
if (pwm_ch4 > left_AT) {
left_spin = true; right_spin = false; no_spin = false;
Serial.println("Spinning Left");
}
if (pwm_ch4 < right_AT) {
right_spin = true; left_spin = false; no_spin = false;
Serial.println("Spinning Right");
}
if ((pwm_ch4 < left_AT) && (pwm_ch4 > right_AT)) {
right_spin = false; left_spin = false; no_spin = true;
Serial.println("Not Spinning");
} Serial.println();
}
/*MEGA BREAD SERIES #7
The CUTSIE WHUN Project
System Voids
Updated 9-18-2017
Google+ Community Page Link: https://plus.google.com/communities/108091771016945233997
*/
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 cycle_counter() {
Serial.println();
Serial.print("Cycle #");
Serial.print(count);
Serial.println();
}
void delayTime() {
delay_time = pwm_ch6 * delayMultiplier;
delay(delay_time);
}
void heartbeat() {
digitalWrite(heartbeat_pin, HIGH);
delay(heartbeatDelay);
digitalWrite(heartbeat_pin, LOW);
}
void setup() {
Serial.begin(SERIAL_PORT_SPEED);
pinMode(LF_motor_pin, OUTPUT);
pinMode(LR_motor_pin, OUTPUT);
pinMode(RF_motor_pin, OUTPUT);
pinMode(RR_motor_pin, OUTPUT);
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);
enableInterrupt(input, dmpDataReady, CHANGE);
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Serial.println("Using Arduino Wire");
delay(delay_time);
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
Serial.println("Using Arduino FastWire");
#endif
delay(delay_time);
Serial.println("Initializing Internal Gyro MPU-DMP I2C device...");
internal_mpu.initialize();
delay(delay_time);
Serial.println("Testing device connections...");
delay(delay_time);
Serial.println(internal_mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
Serial.println("Initializing DMP...");
devStatus = internal_mpu.dmpInitialize();
delay(delay_time);
Serial.println("Initializing Gyro and Accel Offset values...");
internal_mpu.setXAccelOffset(XAccelOffset); Serial.println("XAccelOffset...");
internal_mpu.setYAccelOffset(YAccelOffset); Serial.println("YAccelOffset...");
internal_mpu.setZAccelOffset(ZAccelOffset); Serial.println("ZAccelOffset...");
delay(delay_time);
internal_mpu.setXGyroOffset(XGyroOffset); Serial.println("XGyroOffset...");
internal_mpu.setYGyroOffset(YGyroOffset); Serial.println("YGyroOffset...");
internal_mpu.setZGyroOffset(ZGyroOffset); Serial.println("ZGyroOffset...");
delay(delay_time);
if (devStatus == 0) {
Serial.println("Enabling DMP..."); internal_mpu.setDMPEnabled(true);
delay(delay_time);
Serial.println("Enabling interrupt detection (Arduino external interrupt 0)...");
delay(delay_time);
mpuIntStatus = internal_mpu.getIntStatus();
Serial.println(F("DMP ready! Waiting for first interrupt...")); dmpReady = true;
delay(delay_time);
packetSize = internal_mpu.dmpGetFIFOPacketSize();
}
else {
Serial.print("DMP Initialization failed (code ");
delay(delay_time);
Serial.print(devStatus);
Serial.println();
}
}
void loop() {
count++;
cycle_counter();
heartbeat();
Ping();
rc_read_values();
set_rc_pwm();
gyro_motor_link();
dmpDataReady();
dmp_loop();
rc_motor_link();
arming_state();
throttle_state();
transmission_state();
turn_state();
spin_state();
Serial.println();
print_rc_values();
Serial.println();
delayTime();
}
Pigeon-Kicker
19 projects • 31 followers
Computer guru from the 80's, currently disabled veteran.
Building this stuffs for my son to learn robotics.
Comments