/* File: Land_Rover_Defender_Wiichuck_3 * Arduino motor shield with ultrasonic ranging sensor and Wii nunchuck * * Change Log: * v1 - Initial version * v2 - Changed robot_distance_sensor() to resolve periodic lack of * responsiveness. The function now ignores any distance further than 25 * inches away by adding a 3.6 ms timeout to the pulseIn() call. * v3 - Added buzzer on pin 6. Control with Wii nunchuck Z button. * * Hardware: * - Arduino Uno Rev 3 - http://arduino.cc/en/Main/ArduinoBoardUno * - Arduino Motor Shield R3 - http://arduino.cc/en/Main/ArduinoMotorShieldR3 * - 6V DC motor is driving the rear axle * - Micro servo (5V) is steering the car * - HC-SR04 ultrasonic ranging sensor * - Wii nunchuck provides control * - Buzzer (3V, two wires) provides horn. * * Configuration and Pins: * - The battery is connected to the Vin and Gnd on the motor shield screw * terminal block with an on/off switch * - The 6V drive motor has two wires, one yellow and one blue. The blue wire * is on (+) for channel A, and the yellow is (-). * - The servo steering motor has three pins, brown is ground, red is 5V and * yellow is PWM on pin 5. * - The ranging sensor has four pins. GND and VCC. White wire connects trig to * pin 4, Green wire connects echo to pin 7. * - The Wii nunchuck uses TWI (I2C). Use the Wire library. Use pin A4 (or SDA) * and A5 (or SCL). * * Motor Shield Pins: * D3 - (Ch A) PWM_A * D5 - TinkerKit Analog Output * D6 - TinkerKit Analog Output * D8 - (Ch B) BRK_B, set HIGH to brake, LOW to spin down * D9 - (Ch A) BRK_A, set HIGH to brake, LOW to spin down * D11 - (Ch B) PWM_B * D12 - (Ch A) DIR_A, Direction, set HIGH or LOW * D13 - (Ch B) DIR_B, Direction, set HIGH or LOW * A0 - (Ch A) SNS_A, Current sensing * A1 - (Ch B) SNS_B, Current sensing * A2 - TinkerKit Analog Input * A3 - TinkerKit Analog Input * *** Note for PWM_A/PWM_B: * Use analogWrite(pin,value) to generate a PWM wave. After a call to * analogWrite() the pin will generate a steady square wave of the specified * duty cycle until the next call to analogWrite() or a call to digitalRead() * or digitalWrite() on the same pin. The frequency of the PWM signal is * approximately 490 Hz. Value is 0 (always off) to 255 (always on). * *** Note for SNS_A/SNS_B: * Measure the current going through the DC motor by reading the SNS_A and * SNS_B pins. On each channel will be a voltage proportional to the measured * current, which can be read as a normal analog input, through the function * analogRead() on the analog input A0 and A1. It is calibrated to be 3.3V * when the channel is delivering its maximum possible current (2A). * * Project Setup * Top Pins, Right to Left: * D0 - Not connected, TTL serial RX * D1 - Not connected, TTL serial TX * D2 - Not Used * D3 - PWM_A * D4 - Ultrasonic ranging sensor trigger (white) * D5 - TinkerKit Analog Output, PWM_S, 180 degree servo for steering * D6 - TinkerKit Analog Output, red wire from horn. * D7 - Ultrasonic ranging sensor echo (green) * D8 - Not Used, BRK_B * D9 - BRK_A, set LOW to free spin * D10 - Red LED in illuminated pushbutton switch * D11 - Not Used, PWM_B * D12 - DIR_A, set HIGH for foward, LOW for reverse * D13 - Not Used, DIR_B * GND - Ground for red LED in illuminated pushbutton switch * AREF - Not Used * SDA - Wii nunchuck SDA or use pin A4 * SCL - Wii nunchuck SCL or use pin A5 * * Bottom Pins, Right to Left: * A5 - Wii nunchuck SCL * A4 - Wii nunchuck SDA * A3 - Not Used, TinkerKit Analog Input * A2 - Not Used, TinkerKit Analog Input * A1 - Not Used, SNS_B * A0 - SNS_A, used to read current on drive motor, Ch A. * Vin - Not Used -- use power block on motor shield * Gnd - Wii nunchuck * Gnd - Ultrasonic ranging sensor ground (black) * 5V - Ultrasonic ranging sensor VCC (red) * 3.3V - Wii nunchuck * RESET - Not Used * IOREF - Not Used */ // If defined then output activity to serial port //#define SERIAL_MONITOR #include <Wire.h> #include <Servo.h> Servo strservo; // create servo object for steering /* Define all constants specific to this application, such as pin numbers. * The convention is to use all capital letters for these names. */ // Pin to read current on ch A motor (3.3V = max) #define SNS_A 0 // Pin to drive ch A motor speed, (0=stop, 255=max) #define PWM_A 3 // Pin to drive ch A motor direction (H = forward) #define DIR_A 12 // Pin to set ch A motor brake mode (L = free spin) #define BRK_A 9 // Pin for steering servo (90 = straight) #define PWM_S 5 // Pin for front ranging sensor trigger #define FRONT_T 4 // Pin for front ranging sensor echo #define FRONT_E 7 // Pin for red LED in pushbutton switch #define LEDPIN 10 // Rapid blink rate to warn something is close to the distance sensor #define BLINK_DANGER 100 // Medium blink rate to warn something is near the distance sensor #define BLINK_WARN 500 // Slow blink rate to warn something is within range of the distance sensor #define BLINK_DETECT 1000 // Slow blink to show we're still working. Nothing in range of distance sensor #define BLINK_CLEAR 2000 // Pin for horn. This is a buzzer. We'll use digitalWrite() to control it. #define HORNPIN 6 /* Initialize all global variables * These variables are used across multiple functions to control the state of * the robot. The number of variables should be minimized. If this code is * ported to a multi-core system, such as a Parallax Propeller board, or to a * multi-tasking environment, use of global variables should be eliminated. */ int str_pos = 90; // variable to store the steering servo position int drv_spd = 0; // variable to store the speed (0 to 255) int drv_dir = LOW; // variable to store direction (H = forward) int front_distance = 0; // Distance sensed by front ranging sensor static uint8_t nunchuck_buf[6]; // status from the Wii nunchuck int led_state = LOW; // If the indicator LED is on or off. unsigned long led_interval = BLINK_CLEAR; // Blink rate for indicator LED in ms /* Function: setup() * The setup function is called only once and should perform all * initialization. This isn't typically so long but we're using this for some * power on testing. */ void setup() { /* The serial output code will only be included if SERIAL_MONITOR has been * defined. Leaving out all the serial output code in this file saves over * 1k bytes of code space and frees up processing for other tasks.On the Uno * we're limited to about 31k. */ #ifdef SERIAL_MONITOR Serial.begin(19200); Serial.println("Starting setup"); #endif // Set pin modes pinMode(BRK_A, OUTPUT); pinMode(DIR_A, OUTPUT); pinMode(PWM_A, OUTPUT); pinMode(LEDPIN, OUTPUT); pinMode(FRONT_T, OUTPUT); pinMode(FRONT_E, INPUT); pinMode(HORNPIN, OUTPUT); // One time initializations led_on(); // turn on the red push button LED, our indicator strservo.attach(PWM_S); // attach steering servo digitalWrite(BRK_A, LOW); // set brake to free spin analogWrite(PWM_A, drv_spd); // set speed digitalWrite(DIR_A, drv_dir); // set direction /* To kickoff (and perform a limited power-on self-test) we will rotate the * steering left and right, then drive backward and forward for a few * centimeters. */ // First we try out the steering servo for (str_pos = 90; str_pos < 180; str_pos += 1) { // go from 90 to 180 degrees in steps of 1 degree strservo.write(str_pos); delay(10); // wait 10 ms for the servo to reach position } led_toggle(); delay(1000); // wait for a second for (str_pos = 180; str_pos > 0; str_pos -= 1) { // go from 180 to 0 degrees in steps of 1 degree strservo.write(str_pos); delay(10); // wait 10 ms for the servo to reach position } led_toggle(); delay(1000); // wait for a second // Return to pointing the wheels straight ahead for (str_pos = 0; str_pos < 90; str_pos += 1) { // go from 0 to 90 degrees in steps of 1 degree strservo.write(str_pos); delay(10); // wait 10 ms for the servo to reach position } #ifdef SERIAL_MONITOR Serial.println("Starting Wii nunchuck initialization"); #endif nunchuck_init(); // send the initialization handshake led_toggle(); delay(1000); // wait for a second nunchuck_get_data(); #ifdef SERIAL_MONITOR nunchuck_print_data(); #endif // Now we try out the drive motor drv_spd = 64; // 256/4 = 64 or one-quarter speed analogWrite(PWM_A, drv_spd); led_toggle(); delay(1000); drv_spd = 0; // Full stop analogWrite(PWM_A, drv_spd); led_toggle(); delay(1000); drv_dir = HIGH; // go forward digitalWrite(DIR_A, drv_dir); drv_spd = 64; // all ahead one quarter speed analogWrite(PWM_A, drv_spd); led_toggle(); delay(1000); drv_spd = 0; // Full stop analogWrite(PWM_A, drv_spd); led_toggle(); delay(1000); // wait one second #ifdef SERIAL_MONITOR Serial.println("Setup complete"); #endif } /* Function: loop() * The loop function is the main loop, called repeatedly until the Arduino is * powered off. */ void loop() { int front_range = 0; delay(100); led_blink(); // See if it's time to turn the LED on or off // Read the nunchuck data nunchuck_get_data(); #ifdef SERIAL_MONITOR nunchuck_print_data(); #endif int joy_x_axis = nunchuck_buf[0]; // go left or right int joy_y_axis = nunchuck_buf[1]; // go forward or backward // Check if Z button is being pressed if ((nunchuck_buf[5] >> 0) & 1) horn_off(); // Button = 1 if not pressed else horn_on(); // Button = 0 if pressed // Handle steering left or right robot_joystick_steering(joy_x_axis); // Handle going forward or backward robot_joystick_speed(joy_y_axis); // Check the front distance sensor for obstacles front_range = robot_distance_sensor(FRONT_T, FRONT_E); if ( 6 > front_range ) led_blink_rate(BLINK_DANGER); // something is within 6 inches else if ( 12 > front_range ) led_blink_rate(BLINK_WARN); // something is within 12 inches else if ( 24 > front_range ) led_blink_rate(BLINK_DETECT); // something is within 2 feet else led_blink_rate(BLINK_CLEAR); // nothing to worry about yet } /* Function: robot_joystick_steering(int joy_x_axis) * * Act on steering commands from nunchuck joystick sensor. The value of the x * axis ranges from left = 24, center = 120, right = 221. * The front wheel servo uses left = 0, center = 90, wheels right = 180. * We must map joy_x_axis to a new value for str_pos. * * A simple approach is to right shift by 5 bits, then set str_pos to one of * seven values based on the result. This gives us: * X (binary) : >>5 : str_pos * 24 : 0001 1000 : 000 : 0 * 31 : 0001 1111 : 000 : 0 * 32 : 0010 0000 : 001 : 30 * 63 : 0011 1111 : 001 : 30 * 64 : 0100 0000 : 010 : 60 * 95 : 0101 1111 : 010 : 60 * 96 : 0110 0000 : 011 : 90 * 127 : 0111 1111 : 011 : 90 * 128 : 1000 0000 : 100 : 120 * 159 : 1001 1111 : 100 : 120 * 160 : 1010 0000 : 101 : 150 * 191 : 1011 1111 : 101 : 150 * 192 : 1100 0000 : 110 : 180 * 221 : 1101 1101 : 110 : 180 * This is almost perfect but 120 is the value of the joystick when centered. * The center of our 96-127 range is 112. To adjust for this we must subtract 8 * before we right shift. Otherwise our car will tend to steer right. */ void robot_joystick_steering(int joy_x_axis) { /* Switch statements are easy to understand. A good compiler can optimize * this into small, fast code. I'm not sure how good the Arduino and AVR * compiler are at this yet. * * The switch statement determines what state we should be in by calculating * the new state x where x = (joy_x_axis - 8) >> 5. This takes the current * sensor reading of the x axis, adjusts for our different center points, * then drops the least significant five bits to arrive at one of 7 states. * * Other approaches would be to multiply x by 30. Or if the processor did not * have a multiply operation we could execute a loop to add 30 and decrement * x until x reaches 0. Since x only ranges from 0 to 6 this will be faster * than doing a multiply even on some processors that support integer * multiplication. * * We don't check to see if the new value for str_pos is the same as the old * value. We just update and write to the servo. Recall that the servo is * controlled by a PWM signal. Writing a value to the servo only changes an * internal register used by the PWM hardware in the processor chip to * control the duty cycle of the square wave. The PWM signal is constantly * being sent anyway so changing the register doesn't require additional * power. Effectively, a check for any change to str_pos is being handled for * us in hardware. */ switch ( (joy_x_axis - 8) >> 5 ) { case 0: str_pos = 0; break; case 1: str_pos = 30; break; case 2: str_pos = 60; break; case 3: str_pos = 90; break; case 4: str_pos = 120; break; case 5: str_pos = 150; break; default: str_pos = 180; } strservo.write(str_pos); } /* Function: robot_joystick_speed(int joy_y_axis) * * Act on forward/backward and speed commands from the nunchuck joystick. * The observed y value range is full reverse = 28, center = 130, full forward * = 224. Our direction is controlled by DIR_A with HIGH = forward. We keep * the current value in the global variable drv_dir. Our speed is controlled * by PWM_A (0=stop, 255=max). We keep the current value in the global variable * drv_spd. We must map joy_y_axis to new values for drv_spd and drv_dir. * * A simple approach is to right shift by 5 bits, then set drv_spd and drv_dir * to specific values based on the results. The problem is that if we are going * forward fast and try to go in reverse fast we'll draw too much current, * power will drop below the minimum, and the robot will reboot. * * The solution is to keep track of what state we're in and only allow a * transition to an adjacent state. This way we never make a big change and * draw too much current. As long as the main loop doesn't call this function * too frequently. A delay of about 100 ms in the main loop seems to avoid a * reboot. * * Y (binary) : >>5 : drv_dir : drv_spd * 24 : 0001 1000 : 000 : L : 255 * 31 : 0001 1111 : 000 : L : 255 * 32 : 0010 0000 : 001 : L : 170 * 63 : 0011 1111 : 001 : L : 170 * 64 : 0100 0000 : 010 : L : 85 * 95 : 0101 1111 : 010 : L : 85 * 96 : 0110 0000 : 011 : L : 0 * 127 : 0111 1111 : 011 : L : 0 * 128 : 1000 0000 : 100 : H : 85 * 159 : 1001 1111 : 100 : H : 85 * 160 : 1010 0000 : 101 : H : 170 * 191 : 1011 1111 : 101 : H : 170 * 192 : 1100 0000 : 110 : H : 255 * 221 : 1101 1101 : 110 : H : 255 * * This is almost perfect but 130 is the value of the joystick when centered. * The center of our 96-127 range is 112. To adjust for this we must subtract * 18 before we right shift. */ void robot_joystick_speed(int joy_y_axis) { int y, next_y; static int last_y=3; // Figure out what state the user wants us in. y = (joy_y_axis - 18) >> 5; /* Figure out what state we're able to reach. Only move one state closer to * what the user requested. If we're already there then take no action. */ next_y = last_y; if (y > last_y) next_y = last_y + 1; if (y < last_y) next_y = last_y - 1; // Only do something if we're in a new state, otherwise skip to end. if ( next_y != last_y ) { // Determine new values for direction and speed switch (next_y) { case 0: drv_dir = LOW; drv_spd = 255; break; case 1: drv_dir = LOW; drv_spd = 170; break; case 2: drv_dir = LOW; drv_spd = 85; break; case 3: drv_dir = LOW; drv_spd = 0; break; case 4: drv_dir = HIGH; drv_spd = 85; break; case 5: drv_dir = HIGH; drv_spd = 170; break; default: drv_dir = HIGH; drv_spd = 255; } digitalWrite(DIR_A, drv_dir); analogWrite(PWM_A, drv_spd); last_y = next_y; } } /* Function: robot_distance_sensor(trig_pin, echo_pin) * Determine the distance to object in front of an ultrasonic sensor. * * The following is front the ping example included with the Arduino: * * According to Parallax's datasheet for the PING))), there are 73.746 * microseconds per inch (i.e. sound travels at 1130 feet per second). This * gives the distance travelled by the ping, outbound and return, so we divide * by 2 to get the distance of the obstacle. * See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf * * We don't care about any distance less than 1 inch, and we return an integer * so we can't return fractional distance. To keep this simple we always return * a positive whole number. We should never return 0. * * We also don't care about any distance further than 24 inches. So we set the * pulseIn timeout to 25 * 72 * 2 microseconds, or 3.6 ms. */ int robot_distance_sensor(const int trig_pin, const int echo_pin) { int duration; digitalWrite(trig_pin, LOW); delayMicroseconds(2); digitalWrite(trig_pin, HIGH); delayMicroseconds(5); digitalWrite(trig_pin, LOW); // Only wait long enough to detect up to 25 inches. duration = pulseIn(echo_pin, HIGH, (25 * 72 * 2) ); // Always return a positive integer between 1 and 25). if ( (74 * 2) < duration ) return duration / (74 * 2); else return 25; } /* Some LED indicator helper functions. * The red LED is part of the on/off switch so we know it's visible. The * typical indicator LED is the yellow SMD one mounted on the Arduino board. * However, this is covered by the motor shield and pin 13 is used for channel * B motor control. */ // Turn the indicator LED on void led_on() { digitalWrite(LEDPIN, HIGH); led_state = HIGH; } // Turn the indicator LED off void led_off() { digitalWrite(LEDPIN, LOW); led_state = LOW; } // Toggle the indicator LED void led_toggle() { if ( led_state == HIGH ) led_off(); else led_on(); } /* Function: led_blink() * See http://www.arduino.cc/en/Tutorial/BlinkWithoutDelay * Blink the LED at some rate set by the global variable led_interval. * * After checking if it's time to blink we set last_time = time. We could make * another call to millis() for more accuracy, but at 16MHz and a maximum of 4 * cycles per instruction, 1 ms comes out to at least 4,000 instructions. This * function should be much less than 4k instructions, so there is no effective * additional accuracy. */ void led_blink() { static unsigned long last_time = 0; // This returns how long we've been running as a 32-bit integer unsigned long time = millis(); // Handle the overflow, which happens once every 50 days or so. if ( time < last_time ) last_time = 0xffffffff - last_time; // See if it's time to blink if ( time - last_time > led_interval ) { last_time = time; led_toggle(); } } /* Function: led_blink_rate() * Set the rate at which to blink */ void led_blink_rate( unsigned long interval) { led_interval = interval; led_blink(); } /* Some horn functions. * The buzzer is connected to pin 6, HORNPIN. * Horn on sets the pin high, or 5V. * Horn off sets the pin low, or 0V. */ void horn_on() { digitalWrite(HORNPIN, HIGH); } void horn_off() { digitalWrite(HORNPIN, LOW); } /* Nunchuck Functions * From: NunchuckPrint.ino * 2007 Tod E. Kurt, http://todbot.com/blog/ * * CHANGES: What needed to be changed in the downloaded code * - In Arduino v1.0 Wire.print() became Wire.write(), and Wire.receive() * became Wire.read(). */ // initialize the I2C system, join the I2C bus, and tell the nunchuck we're // talking to it. void nunchuck_init() { Wire.begin(); // join i2c bus as master Wire.beginTransmission(0x52); // transmit to device 0x52 Wire.write(0x40); // sends memory address Wire.write(0x00); // sends sent a zero. Wire.endTransmission(); // stop transmitting } // Send a request for data to the nunchuck // was "send_zero()" void nunchuck_send_request() { Wire.beginTransmission(0x52); // transmit to device 0x52 Wire.write(0x00); // sends one byte Wire.endTransmission(); // stop transmitting } // Receive data back from the nunchuck, int nunchuck_get_data() { int cnt=0; Wire.requestFrom (0x52, 6); // request data from nunchuck while (Wire.available ()) { // receive byte as an integer nunchuck_buf[cnt] = nunchuk_decode_byte(Wire.read()); cnt++; } nunchuck_send_request(); // send request for next data payload // If we recieved the 6 bytes, then go print them if (cnt >= 5) { return 1; // success } return 0; //failure } // Print the input data we have recieved // accel data is 10 bits long so we read 8 bits, then we have to add // on the last 2 bits. That is why I multiply them by 2 * 2 void nunchuck_print_data() { static int i=0; int joy_x_axis = nunchuck_buf[0]; int joy_y_axis = nunchuck_buf[1]; int accel_x_axis = nunchuck_buf[2]; // * 2 * 2; int accel_y_axis = nunchuck_buf[3]; // * 2 * 2; int accel_z_axis = nunchuck_buf[4]; // * 2 * 2; int z_button = 0; int c_button = 0; // byte nunchuck_buf[5] contains bits for z and c buttons // it also contains the least significant bits for the accelerometer data // so we have to check each bit of byte outbuf[5] if ((nunchuck_buf[5] >> 0) & 1) z_button = 1; if ((nunchuck_buf[5] >> 1) & 1) c_button = 1; if ((nunchuck_buf[5] >> 2) & 1) accel_x_axis += 2; if ((nunchuck_buf[5] >> 3) & 1) accel_x_axis += 1; if ((nunchuck_buf[5] >> 4) & 1) accel_y_axis += 2; if ((nunchuck_buf[5] >> 5) & 1) accel_y_axis += 1; if ((nunchuck_buf[5] >> 6) & 1) accel_z_axis += 2; if ((nunchuck_buf[5] >> 7) & 1) accel_z_axis += 1; Serial.print(i,DEC); Serial.print("\t"); Serial.print("joy:"); Serial.print(joy_x_axis,DEC); Serial.print(","); Serial.print(joy_y_axis, DEC); Serial.print(" \t"); Serial.print("acc:"); Serial.print(accel_x_axis, DEC); Serial.print(","); Serial.print(accel_y_axis, DEC); Serial.print(","); Serial.print(accel_z_axis, DEC); Serial.print("\t"); Serial.print("but:"); Serial.print(z_button, DEC); Serial.print(","); Serial.print(c_button, DEC); Serial.print("\r\n"); // newline i++; } // Encode data to format that most wiimote drivers except // only needed if you use one of the regular wiimote drivers char nunchuk_decode_byte (char x) { x = (x ^ 0x17) + 0x17; return x; } // vim: set expandtab tabstop=2 shiftwidth=2 syntax=c: