Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#define GROUP 1

#define MOTOR 0

#define ENCODER 132

void packet_receive (char type, int source, char* packet, int length);

void ctrl(void);

PacketGroupHandler packetHandler = {GROUP, NULL, NULL, &packet_receive, NULL};

char buf[32];

long unsigned int count = 0;

volatile int left_dir = FORWARD;

volatile int right_dir = FORWARD;

volatile int left_ref;

volatile int right_ref;

volatile int left_old;

volatile int right_old;

volatile int left_i = 0;

volatile int right_i = 0;

volatile int left_u;

volatile int right_u;

volatile int left_vel = 0;

volatile int right_vel = 0;

#define PL        1

#define DL        0.1

#define IL        0.1

#define PR        1.5

#define DR        0.2

#define IR        0.1

#define INTERVAL        50

/* receive motor speed from master: 

 * left_speed left_dir right_speed right_dir

 * then sent raw encoder values back to master: 

 * high_left low_left high_right low_right 

 */

void packet_receive (char type, int source, char* packet, int length) {

        if (type != MOTOR) return;

        left_ref = packet[0];

        right_ref = packet[1];

        sprintf(buf, "received: %d %d\n", left_ref, right_ref);

        usb_puts(buf);

        //orb_set_color(GREEN);

}

void ctrl() {

        int left_err = left_ref - left_vel;

        int right_err = right_ref - right_vel;

        left_err = left_vel == 2048 ? left_old : left_err;

        right_err = right_vel == 2048 ? right_old : right_err;

        left_i += left_err;

        left_u = (int)(PL*(float)left_err + DL*(float)(left_err - left_old) + IL*(float)left_i);

        left_old = left_err;

        right_i += right_err;

        right_u = (int)(PR*(float)right_err + DR*(float)(right_err - right_old) + IR*(float)right_i);

        right_old = right_err;

        if (left_u < 0) {

                left_dir = 0;

                left_u = -left_u;

        }

        else

                left_dir = 1;

        if (right_u < 0) {

                right_u = -right_u;

                right_dir = 0;

        }

        else

                right_dir = 1;

        left_u += 160;

        right_u += 160;

        left_u = left_u > 210 ? 210 : left_u;

        right_u = right_u > 210 ? 210 : right_u;

}

int main (void) {

        char encoder[2];

        dragonfly_init(ALL_ON);

        usb_init();

        encoders_init();

        //usb_puts("xbee id ");

        //usb_putc(wl_get_xbee_id());

        wl_init();

    left_ref = 20;

        right_ref = 20;

        wl_register_packet_group(&packetHandler);

        //orb_set_color(RED);

        //motor1_set(FORWARD,230);

        //motor2_set(FORWARD,230);

        while (1) {

                usb_puts("We Win\n");

            //left_vel = encoder_get_v(0);

                //right_vel = encoder_get_v(1);

                 delay_ms(1000);

                //if (!(count % INTERVAL)) {

                //        wl_send_global_packet(GROUP, ENCODER, encoder, 2, 0);

                        encoder[0] = left_vel;

                        encoder[1] = right_vel;

                   // usb_puti(left_vel);

                  //  sprintf(buf, "sent (%d): %d %d %d %d\n", count, left_vel, right_vel, encoder[0], encoder[1]);

                   // usb_puts(buf);

                   // usb_puti(encoder[0]);

          //  }

                //ctrl();

          //  if (!(count % INTERVAL))

          //          wl_do();

//                sprintf(buf, "motor (%d): %d %d %d %d\n", count, left_dir, left_ref, right_dir, right_ref);

//                usb_puts(buf);

            //motor1_set(left_dir, left_u);

                //motor2_set(right_dir, right_u);

                count++;

            delay_ms(1000);

        }

}