Pigeon-Kicker
Published © MIT

Cutsie Whun - I Want to Break You - Durability Test

As with anything that you build, you have to test it, most of the tests are relatively safe and harmless. This test is an actual attempt.

AdvancedWork in progressOver 6 days250
Cutsie Whun - I Want to Break You - Durability Test

Story

Read more

Code

CUTSIE_WHUN_V1.ino

C/C++
The full version code, except gyro linking actions.
#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();
}

Credits

Pigeon-Kicker

Pigeon-Kicker

19 projects • 21 followers
Computer guru from the 80's, currently disabled veteran. Building this stuffs for my son to learn robotics.

Comments