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_err_old;

volatile int right_err_old;

volatile int left_vel_old = 0;

volatile int right_vel_old = 0;

volatile int left_i = 0;

volatile int right_i = 0;

volatile int left_u;

volatile int right_u;

volatile int left_vel;

volatile int right_vel;

#define PL        1

#define DL        0.1

#define IL        0.1

/*

#define PR        1.5

#define DR        0.2

#define IR        0.1

*/

#define PR        1

#define DR        0.1

#define IR        0.1

#define INTERVAL        20

/* 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];

        if (left_ref > 127)

                left_ref -= 256;

        right_ref = packet[1];

        if (right_ref > 127)

                right_ref -= 256;

//        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;

        if (!left_ref)

                left_err = 0;

        if (!right_ref)

                right_err = 0;

//        left_err = left_vel == 2048 ? left_err_old : left_err;

//        right_err = right_vel == 2048 ? right_err_old : right_err;

        left_i += left_err;

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

        left_err_old = left_err;

        right_i += right_err;

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

        right_err_old = right_err;

//        sprintf(buf, "u: %d %d\n", left_u, right_u);

//        usb_puts(buf);

        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;

        if (!left_ref)

                left_u = 0;

        if (!right_ref)

                right_u = 0;

}

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

        wl_register_packet_group(&packetHandler);

        //orb_set_color(RED);

        //motor1_set(FORWARD,230);

        //motor2_set(FORWARD,230);

        while (1) {

                left_vel = encoder_get_v(0);

                right_vel = encoder_get_v(1);

                // err handling

                if (left_vel == 2048)

                        left_vel = left_vel_old;

                else 

                        left_vel_old = left_vel;

                if (right_vel == 2048)

                        right_vel = right_vel_old;

                else 

                        right_vel_old = right_vel;

                if (!(count % INTERVAL)) {

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

                        encoder[0] = left_vel;

                        encoder[1] = right_vel;

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

                        //usb_puts(buf);

                }        

                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(100);

        }

}