Components and supplies
ZX Distance and Gesture Sensor
SparkFun Motor Driver - Dual TB6612FNG (1A)
Voltage Regulator Module
SparkFun Triple Axis Accelerometer and Gyro Breakout - MPU-6050
Arduino Pro Mini 328 - 5V/16MHz
Tools and machines
Hot glue gun (generic)
Soldering iron (generic)
Apps and platforms
Arduino IDE
Project description
Code
Cutsie Whun v2
arduino
1#include <EnableInterrupt.h> 2#include <NewPing.h> 3#include "I2Cdev.h" 4#include "MPU6050.h" 5#include "Wire.h" 6 7#define SERIAL_PORT_SPEED 57600 8#define TRIGGER_PIN 3 9#define ECHO_PIN 4 10#define MAX_DISTANCE 500 11NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); 12#define OUTPUT_READABLE_ACCELGYRO 13MPU6050 accelgyro; 14#define heartbeat_pin 7 15int heartbeatDelay=10; 16int delayTime=0; 17int count = 0; 18int delayMultiplier=2; 19int16_t ax, ay, az; 20int16_t gx, gy, gz; 21int16_t ax1, ay1, az1; 22int16_t gx1, gy1, gz1; 23int x, x1; 24int y, y1; 25int z, z1; 26int temp=0; 27int tempC=0; 28int tempF=0; 29#define RC_NUM_CHANNELS 6 30#define RC_CH1 0 31#define RC_CH2 1 32#define RC_CH3 2 33#define RC_CH4 3 34#define RC_CH5 4 35#define RC_CH6 5 36#define RC_CH1_INPUT A0 37#define RC_CH2_INPUT A1 38#define RC_CH3_INPUT A2 39#define RC_CH4_INPUT A3 40#define RC_CH5_INPUT 8 41#define RC_CH6_INPUT 9 42uint16_t rc_values[RC_NUM_CHANNELS]; 43uint32_t rc_start[RC_NUM_CHANNELS]; 44volatile uint16_t rc_shared[RC_NUM_CHANNELS]; 45 46bool left_turn = false; 47bool right_turn = false; 48bool left_spin = false; 49bool right_spin = false; 50bool not_spinning=true; 51bool straight = true; 52bool reverse = false; 53bool forward = false; 54bool brakes = false; 55bool neutral = true; 56bool armed = false; 57 58int rc_input_Deadzone = 20; 59int rc_input_MIN = 1000; 60int rc_input_MAX = 2000; 61 62int pwm_MIN = 0; 63int pwm_MAX = 255; 64 65int pwm_ch1 = 0; 66int pwm_ch2 = 0; 67int pwm_ch3 = 0; 68int pwm_ch4 = 0; 69int pwm_ch5 = 0; 70int pwm_ch6 = 0; 71 72int arming_MIN = 20; 73int arming_MAX = 230; 74int forward_AT = 150; 75int reverse_AT = 90; 76int left_AT = 150; 77int right_AT = 90; 78int throttle=0; 79 80int LF_motor_pin = 5; 81int LR_motor_pin = 6; 82int RF_motor_pin = 10; 83int RR_motor_pin = 11; 84 85int L_motor_speed = 0; 86int R_motor_speed = 0; 87int outMax = 255; 88int outMin = 0; 89 90float lastInput = 0; 91double ITerm = 0; 92double kp = 2; // Proportional value 93double ki = 0; // Integral value 94double kd = 0; // Derivative value 95double Setpoint = 0; // Initial setpoint is 0 96double Compute(double input) { 97 double error = Setpoint - input; 98 ITerm += (ki * error); 99 if (ITerm > outMax) ITerm = outMax; 100 else if (ITerm < outMin) ITerm = outMin; 101 double dInput = (input - lastInput); 102 // Compute PID Output 103 double output = kp * error + ITerm + kd * dInput; 104 if (output > outMax) output = outMax; 105 else if (output < outMin) output = outMin; 106 // Remember some variables for next time 107 lastInput = input; 108 return output; 109} 110 111void SetSetpoint(double d) { 112 Setpoint = d; 113} 114double GetSetPoint() { 115 return Setpoint; 116} 117 118void rc_read_values() { 119 noInterrupts(); 120 memcpy(rc_values, (const void *)rc_shared, sizeof(rc_shared)); 121 interrupts(); 122} 123 124void calc_input(uint8_t channel, uint8_t input_pin) { 125 if (digitalRead(input_pin) == HIGH) { 126 rc_start[channel] = micros(); 127 } else { 128 uint16_t rc_compare = (uint16_t)(micros() - rc_start[channel]); 129 rc_shared[channel] = rc_compare; 130 } 131} 132 133void calc_ch1() { 134 calc_input(RC_CH1, RC_CH1_INPUT); 135 if (rc_values[RC_CH1] < rc_input_MIN) { 136 rc_values[RC_CH1] = rc_input_MIN; 137 } 138 if (rc_values[RC_CH1] > rc_input_MAX) { 139 rc_values[RC_CH1] = rc_input_MAX; 140 } 141} 142void calc_ch2() { 143 calc_input(RC_CH2, RC_CH2_INPUT); 144 if (rc_values[RC_CH2] < rc_input_MIN) { 145 rc_values[RC_CH2] = rc_input_MIN; 146 } 147 if (rc_values[RC_CH2] > rc_input_MAX) { 148 rc_values[RC_CH2] = rc_input_MAX; 149 } 150} 151void calc_ch3() { 152 calc_input(RC_CH3, RC_CH3_INPUT); 153 if (rc_values[RC_CH3] < rc_input_MIN) { 154 rc_values[RC_CH3] = rc_input_MIN; 155 } 156 if (rc_values[RC_CH3] > rc_input_MAX) { 157 rc_values[RC_CH3] = rc_input_MAX; 158 } 159} 160void calc_ch4() { 161 calc_input(RC_CH4, RC_CH4_INPUT); 162 if (rc_values[RC_CH4] < rc_input_MIN) { 163 rc_values[RC_CH4] = rc_input_MIN; 164 } 165 if (rc_values[RC_CH4] > rc_input_MAX) { 166 rc_values[RC_CH4] = rc_input_MAX; 167 } 168} 169void calc_ch5() { 170 calc_input(RC_CH5, RC_CH5_INPUT); 171 if (rc_values[RC_CH5] < rc_input_MIN) { 172 rc_values[RC_CH5] = rc_input_MIN; 173 } 174 if (rc_values[RC_CH5] > rc_input_MAX) { 175 rc_values[RC_CH5] = rc_input_MAX; 176 } 177} 178void calc_ch6() { 179 calc_input(RC_CH6, RC_CH6_INPUT); 180 if (rc_values[RC_CH6] < rc_input_MIN) { 181 rc_values[RC_CH6] = rc_input_MIN; 182 } 183 if (rc_values[RC_CH6] > rc_input_MAX) { 184 rc_values[RC_CH6] = rc_input_MAX; 185 } 186} 187void print_rc_values() { 188 Serial.print(" Right Stick Horizontal:\ "); Serial.print("Remote CH1\ "); Serial.print(rc_values[RC_CH1]); Serial.print("-"); Serial.println(pwm_ch1); 189 Serial.print(" Right Stick Verticle:\ "); Serial.print("Remote CH2\ "); Serial.print(rc_values[RC_CH2]); Serial.print("-"); Serial.println(pwm_ch2); 190 Serial.print(" Left Stick Horizontal:\ "); Serial.print("Remote CH4\ "); Serial.print(rc_values[RC_CH4]); Serial.print("-"); Serial.println(pwm_ch4); 191 Serial.print(" Left Stick Verticle:\ "); Serial.print("Remote CH3\ "); Serial.print(rc_values[RC_CH3]); Serial.print("-"); Serial.println(pwm_ch3); 192 Serial.print(" Arming switch signal:\ "); Serial.print("Remote CH5\ "); Serial.print(rc_values[RC_CH5]); Serial.print("-"); Serial.println(pwm_ch5); 193 Serial.print(" Mode Dial(Delay time):\ "); Serial.print("Remote CH6\ "); Serial.print(rc_values[RC_CH6]); Serial.print("-"); Serial.println(pwm_ch6); 194} 195 196void set_rc_pwm() { 197 pwm_ch1 = map(rc_values[RC_CH1], rc_input_MIN, rc_input_MAX, pwm_MIN, pwm_MAX); 198 if (pwm_ch1 < pwm_MIN) { pwm_ch1 = pwm_MIN; } 199 if (pwm_ch1 > pwm_MAX) { pwm_ch1 = pwm_MAX; } 200 pwm_ch2 = map(rc_values[RC_CH2], rc_input_MIN, rc_input_MAX, pwm_MIN, pwm_MAX); 201 if (pwm_ch2 < pwm_MIN) { pwm_ch2 = pwm_MIN; } 202 if (pwm_ch2 > pwm_MAX) { pwm_ch2 = pwm_MAX; } 203 pwm_ch3 = map(rc_values[RC_CH3], rc_input_MIN, rc_input_MAX, pwm_MIN, pwm_MAX); 204 if (pwm_ch3 < pwm_MIN) { pwm_ch3 = pwm_MIN; } 205 if (pwm_ch3 > pwm_MAX) { pwm_ch3 = pwm_MAX; } 206 pwm_ch4 = map(rc_values[RC_CH4], rc_input_MIN, rc_input_MAX, pwm_MIN, pwm_MAX); 207 if (pwm_ch4 < pwm_MIN) { pwm_ch4 = pwm_MIN; } 208 if (pwm_ch4 > pwm_MAX) { pwm_ch4 = pwm_MAX; } 209 pwm_ch5 = map(rc_values[RC_CH5], rc_input_MIN, rc_input_MAX, pwm_MIN, pwm_MAX); 210 if (pwm_ch5 < pwm_MIN) { pwm_ch5 = pwm_MIN; } 211 if (pwm_ch5 > pwm_MAX) { pwm_ch5 = pwm_MAX; } 212 pwm_ch6 = map(rc_values[RC_CH6], rc_input_MIN, rc_input_MAX, pwm_MIN, pwm_MAX); 213 if (pwm_ch6 < pwm_MIN) { pwm_ch6 = pwm_MIN; } 214 if (pwm_ch6 > pwm_MAX) { pwm_ch6 = pwm_MAX; } 215} 216 217void Ping() { 218 Serial.print("Clear distance to front:\ "); 219 Serial.print(sonar.ping_in()); 220 Serial.print("-In\ "); 221 Serial.print(sonar.ping_cm()); 222 Serial.println("-Cm"); 223} 224 225void delay_time(){ 226 delayTime=pwm_ch6*delayMultiplier; 227 delay(delayTime); 228} 229 230void arming_state() { 231 Serial.print(" System Arming State:\ "); 232 if (pwm_ch5 <= arming_MIN) { 233 armed = true; 234 Serial.println("Armed"); 235 } else if (pwm_ch5 >= arming_MAX) { 236 armed = false; 237 Serial.println("Disarmed"); 238 } 239} 240 241void spin_state(){ 242 Serial.print("\ Spin State:\ "); 243 if (pwm_ch4 > left_AT) { 244 left_spin = true; 245 right_spin = false; 246 not_spinning = false; 247 Serial.println("Spinning Left"); 248 } 249 if (pwm_ch4 < right_AT) { 250 right_spin = true; 251 left_spin = false; 252 not_spinning = false; 253 Serial.println("Spinning Right"); 254 } 255 if ((pwm_ch4 < left_AT) && (pwm_ch4 > right_AT)) { 256 right_spin = false; 257 left_spin = false; 258 not_spinning = true; 259 Serial.println("Not Spinning"); 260 } 261} 262 263void turn_state() { 264 Serial.print("\ Turn State:\ "); 265 if (pwm_ch1 > left_AT) { 266 left_turn = true; 267 right_turn = false; 268 straight = false; 269 Serial.println("Turning Left"); 270 } 271 if (pwm_ch1 < right_AT) { 272 right_turn = true; 273 left_turn = false; 274 straight = false; 275 Serial.println("Turning Right"); 276 } 277 if ((pwm_ch1 < left_AT) && (pwm_ch1 > right_AT)) { 278 right_turn = false; 279 left_turn = false; 280 straight = true; 281 Serial.println("Going Straight"); 282 } 283} 284 285void throttle_state(){ 286 Serial.print(" Throttle State: \ "); 287 throttle=map(pwm_ch3, pwm_MIN, pwm_MAX, 0, 100); 288 Serial.print(throttle);Serial.println("%"); 289} 290 291void transmission_state() { 292 Serial.print(" Transmission State: \ "); 293 if (pwm_ch2 < reverse_AT) { 294 reverse = true; 295 forward = false; 296 neutral = false; 297 Serial.print("Reverse"); 298 } 299 if (pwm_ch2 > forward_AT) { 300 forward = true; 301 reverse = false; 302 neutral = false; 303 Serial.print("Forward"); 304 } 305 if (pwm_ch2 < forward_AT && pwm_ch2 > reverse_AT) { 306 reverse = false; 307 forward = false; 308 neutral = true; 309 Serial.print("Neutral"); 310 } 311 Serial.println(); 312} 313 314void callGyro() { 315 accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); 316 accelgyro.getAcceleration(&ax1, &ay1, &az1); 317 accelgyro.getRotation(&gx1, &gy1, &gz1); 318 319 Serial.print("Gyro Motion-Acceleration:\ "); 320 Serial.print("AX"); Serial.print(ax); Serial.print("\ "); 321 Serial.print("\ AY"); Serial.print(ay); Serial.print("\ "); 322 Serial.print("\ AZ"); Serial.print(az); Serial.println("\ "); 323 Serial.print(" Gyro Motion-Rotation:\ "); 324 Serial.print("GX"); Serial.print(gx); Serial.print("\ "); 325 Serial.print("\ GY"); Serial.print(gy); Serial.print("\ "); 326 Serial.print("\ GZ"); Serial.println(gz); 327 Serial.print(" Gyro Acceleration:\ "); 328 Serial.print("AX"); Serial.print(ax1); Serial.print("\ "); 329 Serial.print("\ AY"); Serial.print(ay1); Serial.print("\ "); 330 Serial.print("\ AZ"); Serial.println(az1); 331 Serial.print(" Gyro Rotation:\ "); 332 Serial.print("GX"); Serial.print(gx1); Serial.print("\ "); 333 Serial.print("\ GY"); Serial.print(gy1); Serial.print("\ "); 334 Serial.print("\ GZ"); Serial.println(gz1); 335} 336 337void get_temp() { 338temp=accelgyro.getTemperature(); 339tempC=temp/340.00+36.53; 340tempF=(temp/340.00+36.53)*1.8+32; 341Serial.print("\ Temperature:\ ");Serial.print(tempC);Serial.print("-C\ ");Serial.print(tempF);Serial.println("-F"); 342} 343 344void rc_motor_link() { 345 if ((reverse = true) && (armed = true)) { 346 analogWrite(LR_motor_pin, L_motor_speed); 347 analogWrite(RR_motor_pin, R_motor_speed); 348 analogWrite(LF_motor_pin, pwm_MIN); 349 analogWrite(RF_motor_pin, pwm_MIN); 350 } 351 if ((forward = true) && (armed = true)) { 352 analogWrite(LF_motor_pin, L_motor_speed); 353 analogWrite(RF_motor_pin, R_motor_speed); 354 analogWrite(LR_motor_pin, pwm_MIN); 355 analogWrite(RR_motor_pin, pwm_MIN); 356 } 357 if (neutral = true) { 358 analogWrite(LR_motor_pin, pwm_MIN); 359 analogWrite(RR_motor_pin, pwm_MIN); 360 analogWrite(LF_motor_pin, pwm_MIN); 361 analogWrite(RF_motor_pin, pwm_MIN); 362 } 363 if (left_turn = true) { 364 analogWrite(LR_motor_pin, pwm_MIN); 365 analogWrite(RR_motor_pin, pwm_MIN); 366 analogWrite(LF_motor_pin, pwm_MIN); 367 analogWrite(RF_motor_pin, R_motor_speed); 368 } 369 if (right_turn = true) { 370 analogWrite(LR_motor_pin, pwm_MIN); 371 analogWrite(RR_motor_pin, pwm_MIN); 372 analogWrite(RF_motor_pin, pwm_MIN); 373 analogWrite(LF_motor_pin, L_motor_speed); 374 } 375} 376void heartbeat() { 377 digitalWrite(heartbeat_pin, HIGH); 378 delay(heartbeatDelay); 379 digitalWrite(heartbeat_pin, LOW); 380} 381 382void setup() { 383 Serial.begin(SERIAL_PORT_SPEED); 384 pinMode(RC_CH1_INPUT, INPUT); 385 pinMode(RC_CH2_INPUT, INPUT); 386 pinMode(RC_CH3_INPUT, INPUT); 387 pinMode(RC_CH4_INPUT, INPUT); 388 pinMode(RC_CH5_INPUT, INPUT); 389 pinMode(RC_CH6_INPUT, INPUT); 390 pinMode(heartbeat_pin, OUTPUT); 391 enableInterrupt(RC_CH1_INPUT, calc_ch1, CHANGE); 392 enableInterrupt(RC_CH2_INPUT, calc_ch2, CHANGE); 393 enableInterrupt(RC_CH3_INPUT, calc_ch3, CHANGE); 394 enableInterrupt(RC_CH4_INPUT, calc_ch4, CHANGE); 395 enableInterrupt(RC_CH5_INPUT, calc_ch5, CHANGE); 396 enableInterrupt(RC_CH6_INPUT, calc_ch6, CHANGE); 397#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 398 Wire.begin(); 399 Serial.println("Using Arduino Wire"); 400#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 401 Fastwire::setup(400, true); 402 Serial.println("Using Arduino FastWire"); 403#endif 404 Serial.println("Initializing I2C devices..."); 405 accelgyro.initialize(); 406 Serial.println("Testing device connections..."); 407 Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); 408 pinMode(LF_motor_pin, OUTPUT); 409 pinMode(LR_motor_pin, OUTPUT); 410 pinMode(RF_motor_pin, OUTPUT); 411 pinMode(RR_motor_pin, OUTPUT); 412 digitalWrite(LF_motor_pin, LOW); 413 digitalWrite(LF_motor_pin, LOW); 414 digitalWrite(LF_motor_pin, LOW); 415 digitalWrite(LF_motor_pin, LOW); 416} 417 418void loop() { 419 heartbeat(); 420 Serial.println(); 421 Serial.print("Cycle #"); 422 Serial.print(count); 423 Serial.println(); 424 Ping(); 425 get_temp(); 426 arming_state(); 427 transmission_state(); 428 turn_state(); 429 spin_state(); 430 throttle_state(); 431 Serial.println(); 432 rc_read_values(); 433 set_rc_pwm(); 434 rc_motor_link(); 435 print_rc_values(); 436 Serial.println(); 437 callGyro(); 438 count++; 439 delay_time(); 440} 441
Downloadable files
Cutsie Whun v2 ino
Cutsie Whun v2 ino
Fritzing Breadboard
Fritzing Breadboard
Cutsie Whun Fritzing
Cutsie Whun Fritzing
Cutsie Whun Schematic
Cutsie Whun Schematic
Cutsie Whun v2 ino
Cutsie Whun v2 ino
Cutsie Whun Schematic
Cutsie Whun Schematic
Fritzing Breadboard
Fritzing Breadboard
Cutsie Whun Fritzing
Cutsie Whun Fritzing
Comments
Only logged in users can leave comments
Pigeon-Kicker
0 Followers
•0 Projects
+1
Work attribution
Table of contents
Intro
6
0