Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#include <wl_defs.h>

#include "../../../common/colonet_defs.h"

#include <serial.h>

#include <stdio.h>

/* Current state codes */

#define TRIGGER_DOWN              1

#define TRIGGER_UP               (1<<1)

#define BUTTON_DOWN              (1<<2)

#define BUTTON_UP                (1<<3)

#define X_LEFT                   (1<<4)

#define X_CENTER                 (1<<5)

#define X_RIGHT                  (1<<6)

#define Y_UP                     (1<<7)

#define Y_CENTER                 (1<<8)

#define Y_DOWN                   (1<<9)

/* State change codes */

#define TRIGGER_PRESSED           1

#define TRIGGER_RELEASED          (1<<1)

#define BUTTON_PRESSED            (1<<2)

#define BUTTON_RELEASED           (1<<3)

#define MOVED_LEFT                (1<<4)

#define MOVED_X_CENTER            (1<<5)

#define MOVED_RIGHT               (1<<6)

#define MOVED_UP                  (1<<7)

#define MOVED_Y_CENTER            (1<<8)

#define MOVED_DOWN                (1<<9)

/* Command Types */

#define CNTL_FORWARD 'f'

#define CNTL_STOP    's'

#define CNTL_BACK    'b'

#define CNTL_LEFT    'l'

#define CNTL_RIGHT   'r'

#define CNTL_BUZZ0   't'

#define CNTL_BUZZ1   'u'

#define WL_CNTL_GROUP 4

int joystick_status (void);

int joystick_change (int old, int new);

PacketGroupHandler cntlHandler = {WL_CNTL_GROUP, NULL, NULL, NULL, NULL};

int main (void) {

          dragonfly_init(SERIAL | USB | COMM | ORB | ANALOG);

        ADCSRA = _BV(ADEN) | _BV(ADPS2) | _BV(ADPS1) | _BV(ADPS0);

          wl_init();

          wl_register_packet_group(&cntlHandler);

          orb_set_color(BLUE);

          int change;

          int last_status = 0; 

          int cur_status = 0;

        while (1) {

                last_status = cur_status;

                cur_status = joystick_status();

                change = joystick_change(last_status, cur_status);

                if (change & MOVED_UP) {

                        orb_set_color(GREEN);

                        wl_send_global_packet(4, CNTL_FORWARD, NULL, 0, 0);

                }

                if (change & MOVED_Y_CENTER) {

                        orb_set_color(RED);

                        wl_send_global_packet(4, CNTL_STOP, NULL, 0, 0);

                }

                if (change & MOVED_DOWN) {

                        orb_set_color(BLUE);

                        wl_send_global_packet(4, CNTL_BACK, NULL, 0, 0);

                }

                if (change & MOVED_LEFT) {

                        orb_set_color(ORANGE);

                        wl_send_global_packet(4, CNTL_LEFT, NULL, 0, 0);

                }

                if (change & MOVED_X_CENTER) {

                        orb_set_color(RED);

                        wl_send_global_packet(4, CNTL_STOP, NULL, 0, 0);

                }

                if (change & MOVED_RIGHT) {

                        orb_set_color(YELLOW);

                        wl_send_global_packet(4, CNTL_RIGHT, NULL, 0, 0);

                }

                if (change & TRIGGER_PRESSED) {

                        orb_set_color(PURPLE);

                        wl_send_global_packet(4, CNTL_BUZZ0, NULL, 0, 0);

                }

                if (change & BUTTON_PRESSED) {

                        orb_set_color(YELLOW);

                        wl_send_global_packet(4, CNTL_STOP, NULL, 0, 0);

                }

                delay_ms(100);

        }

}

int joystick_change (int old, int new) {

        int trigger_change = 0;

        int button_change = 0;

        int x_change = 0;

        int y_change = 0;

        /* Check for trigger change */

        if ((old & TRIGGER_DOWN) && (new & TRIGGER_UP))

                trigger_change = TRIGGER_RELEASED;

        if ((old & TRIGGER_UP) && (new & TRIGGER_DOWN)) 

                trigger_change = TRIGGER_PRESSED;

        /* Check for button change */

        if ((old & BUTTON_DOWN) && (new & BUTTON_UP))

                button_change = BUTTON_RELEASED;

        if ((old & BUTTON_UP) && (new & BUTTON_DOWN))

                button_change = BUTTON_PRESSED;

        /* Check for X change */

        if ((old & X_CENTER) && (new & X_RIGHT))

                x_change = MOVED_RIGHT;

        if ((old & X_CENTER) && (new & X_LEFT))

                x_change = MOVED_LEFT;

        if (!(old & X_CENTER) && (new & X_CENTER))

                x_change = MOVED_X_CENTER;

        /* Check for Y change */

        if ((old & Y_CENTER) && (new & Y_UP))

                y_change = MOVED_UP;

        if ((old & Y_CENTER) && (new & Y_DOWN))

                y_change = MOVED_DOWN;

        if (!(old & Y_CENTER) && (new & Y_CENTER))

                y_change = MOVED_Y_CENTER;

        return trigger_change | button_change | x_change | y_change;

}

/**

 * Get the current status of the joystick.

 * This includes the X and Y axes and the buttons.

 */

int joystick_status (void) {

        int trigger_status = 0;

        int button_status = 0;

        int x_status = 0; 

        int y_status = 0;

        int xval, yval;

        if (analog_get8(AN0) > 200)

                trigger_status = TRIGGER_DOWN;

        else

                trigger_status = TRIGGER_UP;

        if (analog_get8(AN1) > 200)

                button_status = BUTTON_DOWN;

        else

                button_status = BUTTON_UP;

        xval = analog_get8(AN4); // actually AN6, AN7 for connection

        yval = analog_get8(AN5);

        if (xval > 150)

                x_status = X_LEFT;

        else if (xval < 75)

                x_status = X_RIGHT;

        else

                x_status = X_CENTER;

        if (yval > 150)

                y_status = Y_UP;

        else if (yval < 75)

                y_status = Y_DOWN;

        else

                y_status = Y_CENTER;

        return trigger_status | button_status | x_status | y_status;

}