Subversion Repositories group.electronics

#include <Wire.h>

#include <math.h>

#include <VirtualWire.h>

#include "nunchuck_funcs.h"

const int    MFORWARD      = 1;         // Set track to go forward

const int    MREVERSE      = 0;         // Set track to reverse

const float  SKID_FACTOR = 0.8;         // Slow down inner track when skid steering

const float  TURN_FACTOR = 0.33;        // Inner track runs at n the speed of outside track

const int    TURN_MAX    = 102;         // Maximum distance on WiiChuck diagonals

const int    MIN_X      = 25;

const int    CENTER_X   = 125;         // Center of WiiChuck X value

const int    MAX_X      = 225;

const int    MIN_Y      = 30;           // Minimum WiiChuck Y value

const int    CENTER_Y   = 130;          // Center WiiChuck Y value

const int    MAX_Y      = 230;          // Maximum WiiChuck Y value

const int    ZONE_WIDTH  = 40;          // With on zone between movement modes

const int    ZONE_HALF   = ZONE_WIDTH / 2; 

const int    ZONE_X_LOW  = CENTER_X - ZONE_HALF;

const int    ZONE_X_HIGH = CENTER_X + ZONE_HALF;

const int    ZONE_Y_LOW  = CENTER_Y - ZONE_HALF;

const int    ZONE_Y_HIGH = CENTER_Y + ZONE_HALF;

int ledPin = 13;

int loop_cnt=0;

char ttx_buffer[4];

byte joyy,joyx,zbut,cbut;

void setup()

{

    Serial.begin(9600);

    nunchuck_setpowerpins();

    nunchuck_init(); // send the initilization handshake

    vw_setup(2000);	 // VirtualWire Bits per sec  

}

void loop()

{

    if( loop_cnt > 10) { // every 100 msecs get new data

        loop_cnt = 0;

        nunchuck_get_data();

        joyx = nunchuck_joyx();

        joyy = nunchuck_joyy();        

        zbut = nunchuck_zbutton();

        cbut = nunchuck_cbutton();

        // Speed of each track

        byte r_spd = 0;

        byte l_spd = 0;

        // Direction of each track

        byte r_dir = 1;

        byte l_dir = 1;

        byte options = B00000000;

        //              -------0    -> Right track direction ( 1=F, 0=R )

        //              ------0-    -> Left track direction ( 1=F, 0=R )        

        // *1* Reverse && in dead zone of X

        if (joyy <= ZONE_Y_LOW && joyx >= ZONE_X_LOW && joyx <= ZONE_X_HIGH) {

          l_dir = r_dir = MREVERSE;

          r_spd = l_spd = map (joyy, MIN_Y, CENTER_Y, 254, 0);

        // *2* Forward && dead zone of X

        } else if (joyy >= ZONE_Y_HIGH && joyx >= ZONE_X_LOW && joyx <= ZONE_X_HIGH) {

          l_dir = r_dir = MFORWARD;

          r_spd = l_spd = map (joyy, CENTER_Y, MAX_Y, 0, 254);

        // *3* Turn left && dead zone y

        } else if (joyx <= ZONE_X_LOW && joyy >= ZONE_Y_LOW && joyy <= ZONE_Y_HIGH) {

          l_dir = MREVERSE;

          r_dir = MFORWARD;

          r_spd = map (joyx, MIN_X, CENTER_X, 254, 0);

          l_spd = r_spd * SKID_FACTOR;

         // *4* Turn right && dead zone y

        } else if (joyx >= ZONE_X_HIGH && joyy >= ZONE_Y_LOW && joyy <= ZONE_Y_HIGH) {

          l_dir = MFORWARD;

          r_dir = MREVERSE;

          l_spd = map (joyx, CENTER_X, MAX_X, 0, 254);

          r_spd = l_spd * SKID_FACTOR;

         // *5* Forward turn left

        } else if (joyx <= ZONE_X_LOW && joyy >= ZONE_Y_HIGH) {

          // Max (jx:50,jy:205), Dis: 103

          int dx = CENTER_X - joyx;

          int dy = joyy - CENTER_Y;

          int d = sqrt( (dx *dx) + (dy * dy) );

          r_spd = map (d, 0, TURN_MAX, 0, 254);

          l_spd = r_spd * TURN_FACTOR;

          l_dir = r_dir = 1;

         // *6* Forward turn right

        } else if (joyx >= ZONE_X_HIGH && joyy >= ZONE_Y_HIGH) {

          // Max (jx:200,jy:205), Dis: 103

          int dx = joyx - CENTER_X;

          int dy = joyy - CENTER_Y;

          int d = sqrt( dx * dx + dy * dy);

          l_spd = map (d, 0, TURN_MAX, 0, 254);

          r_spd = l_spd * TURN_FACTOR;

          l_dir = r_dir = 1;      

         // *7* Reverse turn right

        } else if (joyx >= ZONE_X_HIGH && joyy <= ZONE_Y_LOW) {

          int dx = joyx - CENTER_X;

          int dy = CENTER_Y - joyy;

          int d = sqrt( dx * dx + dy * dy);

          l_spd = map (d, 0, TURN_MAX, 0, 254);

          r_spd = l_spd * TURN_FACTOR;

          l_dir = r_dir = 0;         

         // *8* Reverse turn left

        } else if (joyx <= ZONE_X_LOW && joyy <= ZONE_Y_LOW) {

          int dx = CENTER_X - joyx;

          int dy = CENTER_Y - joyy;

          int d = sqrt( dx * dx + dy * dy);

          r_spd = map (d, 0, TURN_MAX, 0, 254);

          l_spd = r_spd * TURN_FACTOR;

          l_dir = r_dir = 0; 

         // In dead zone, don't move 

        } else {

          l_dir = r_dir = MFORWARD;

          r_spd = l_spd = 0;

        }

        // Set the options based for track direction

        if (r_dir == MFORWARD)

          bitSet(options, 0);

        if (l_dir == MFORWARD)

          bitSet(options, 1);

        // Prepare the send buffer

        ttx_buffer[0] = options +1;

        ttx_buffer[1] = r_spd + 1;

        ttx_buffer[2] = l_spd + 1;

        ttx_buffer[3] = 0x00;

        Serial.print("opt: "); Serial.print((byte)options,DEC);

        Serial.print("\trspd: "); Serial.print((byte)r_spd,DEC);

        Serial.print("\tlspd: "); Serial.print((byte)l_spd,DEC);

        Serial.println("");

        digitalWrite(13, true); // Flash a light to show transmitting

        vw_send((uint8_t *)ttx_buffer, strlen(ttx_buffer));

        vw_wait_tx(); // Wait until the whole message is gone

        digitalWrite(13, false);    

    }

    loop_cnt++;

    delay(1);

}