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);
}