void state_before_game()
{
m_red(ON);
while(!wireless_buffer_f);
m_red(OFF);
wireless_buffer_f = false;
char inst = wireless_buffer[0];
switch(wireless_buffer[0])
{
case 0xA0:
m_green(ON);
m_red(ON);
m_wait(1000);
m_green(OFF);
m_red(OFF);
m_wait(1000);
m_green(ON);
m_red(ON);
m_wait(1000);
m_green(OFF);
m_red(OFF);
state_before_game();
break;
case 0xA1:
state_play();
break;
default:
state_before_game();
break;
}
}
void init(void)
{
m_green(ON);
set(TCCR1B, WGM13);
set(TCCR1B, WGM12);
set(TCCR1A, WGM11);
set(TCCR1A, WGM10);
OCR1A = 1000;
OCR1C = 100;
set(TCCR1A, COM1C1);
clear(TCCR1A, COM1C0);
set(TIMSK1, TOIE1);
clear(TCCR1B, CS12);
clear(TCCR1B, CS11);
set(TCCR1B, CS10);
m_green(OFF);
}
int main(void) {
m_clockdivide(0);
setup_pins();
if ( RF_debug) {setupUSB();}
setup_timer_1();
setup_timer_3();
m_bus_init();
m_rf_open(chan,RX_add,p_length);
int timer_3_cnt = 0;
//sei();
set_motors(0,0);
while (1){
if (check(TIFR3,OCF3A)){
set(TIFR3, OCF3A);
timer_3_cnt++;
if ( timer_3_cnt % 10 ==0 ) {
timer_3_cnt=0;
m_green(TOGGLE);
}
if ( timer_3_cnt == 1000){ timer_3_cnt=0;}
}
if(new){
switch ( receive_buffer[11] ) {
case Single_Joy: single_joystick(); break;
case Double_Joy: double_joystick(); break;
case Tank_Mode: tank_driving(); break;
case Mario_Kart: Mario_Drive(); break;
default : set_motors(0,0); m_green(2);
}
if(RF_debug){ debug_rf(); }
}
}
}
void goalie(){
init_all();//initializing all the rest
//displaying two flashes to know robot is ready and waiting for start
m_red(ON);
m_green(ON);
m_wait(750);
m_green(OFF);
m_red(OFF);
m_wait(750);
m_red(ON);
m_green(ON);
m_wait(750);
m_red(OFF);
m_green(OFF);
wait(1);
//initializing center ice
set_position(1024/2,768/2);
get_position(blobs, &x_center, &y_center, &theta_zero);
// set voltage reference to 5V
clear(ADMUX, REFS1);
set(ADMUX, REFS0);
m_disableJTAG();//allowing gpio of F pins
//setting ADC prescaler
set(ADCSRA,ADPS2);
set(ADCSRA,ADPS1);
set(ADCSRA,ADPS0);
//setting pins to turn off digital circuitry
set(DIDR0,ADC4D);//setting F4
set(DIDR0,ADC6D);//setting F6
set(DIDR2,ADC8D);//setting D4
set(DIDR2,ADC9D);//setting D6
set(ADCSRA,ADATE);//setting triggering to free running
//setting pins for output & setting pins high to supply power to phototransistors
set(DDRF,5);//F5
set(PORTF,5);
set(DDRF,7);//F7
set(PORTF,7);
set(DDRB,3);//B3
set(PORTB,3);
set(DDRD,3);//D3
set(PORTD,3);
set(DDRF,0);//setting pins to turn the sprinner motors
set(PORTF,0);
set(DDRF,1);
set(PORTF,1);
int adc = 0;//declaring integer for adc status;
int adc_values[4];//declaring adc array
state_before_game();
}
void set_drive_mode( bool L_bump, bool R_bump){
if ( L_bump && R_bump ) { Mario = true;}
else if (!L_bump && R_bump ) { tank_mode = true;}
else if ( L_bump && !R_bump ) { double_joy = true;}
else if ( !L_bump && !R_bump ) { single_joy = true;}
if ( Mario ) { send_buffer[11] = 64; m_red(1); m_green(1);}
else if ( tank_mode ) { send_buffer[11] =100; m_red(1); m_green(0);}
else if ( double_joy ) { send_buffer[11] = 11; m_red(0); m_green(1);}
else if ( single_joy ) { send_buffer[11] = 1; m_red(0); m_green(0);}
}
void event_comm_test()
{
for(int i = 0; i < 5; i++)
{
m_green(ON);
m_red(ON);
m_wait(100);
m_green(OFF);
m_red(OFF);
m_wait(100);
}
}
int main(void)
{
m_clockdivide(0);
m_bus_init();
yes = m_port_init(ADDR);
int flag = 0;
set(DDRD,7);
m_port_set(ADDR,DDRH,7);
while(1)
{
if (yes){
clear(PORTD,7);
}
else {
set(PORTD,7);
}
m_wait(500);
if (flag == 0) {
m_green(TOGGLE);
m_port_set(ADDR,PORTH,7);
set(PORTD,7);
flag = 1;
} else {
m_port_clear(ADDR,PORTH,7);
clear(PORTD,7);
flag = 0;
}
}
}
int main(void)
{
m_red(ON);
m_usb_init();
while(!m_usb_isconnected()); // wait for a serial msg
m_red(OFF);
m_clockdivide(0); // set clock frequency to 16MHz
adc_setup();
m_disableJTAG();
set(ADCSRA,ADEN);//enable ADC
sei();//enable global interrupts
set(ADCSRA,ADSC);//start conversion
while(1){
if(FLAG){ // compare readings and move motor accordingly
m_green(TOGGLE);
clear(ADCSRA,ADEN); // Disable ADC
m_usb_tx_string("\nF0:");
m_usb_tx_int(ir[0]);
m_usb_tx_string("\tF1: ");
m_usb_tx_int(ir[1]);
m_usb_tx_string("\tF4: ");
m_usb_tx_int(ir[2]);
FLAG = 0;
set(ADCSRA,ADEN); // enable ADC again
set(ADCSRA,ADSC);//start next conversion
}
}
}
/************************************************************
Main Loop
************************************************************/
int main(void)
{
/* Confirm Power */
m_red(ON);
/* Initializations */
initialize_robockey();
pause();
play();
/* Confirm successful initialization(s) */
m_green(ON);
/* Run */
while (1){
update_position();
if (check(ADCSRA,ADIF)){adc_update();} // Check if ADC conversion has completed
bot_behavior_update();
if (check(TIFR3,OCF3A)){motor_update();} // Check if timestep has completed
if (wifi_flag) {
wifi_flag = 0;
m_red(TOGGLE);
wireless_recieve();
}
}
}
void process_command(void)
{
if ( ! TWI_Transceiver_Busy() )
{
if ( TWI_statusReg.RxDataInBuf )
{
TWI_Get_Data_From_Transceiver(messageBuf, 1);
switch (messageBuf[0])
{
case M_BLUE_TOGGLE:
m_blue(TOGGLE);
unsigned char twi_data_1[7];
twi_data_1[0]=M_BLUE_TOGGLE;
twi_data_1[1]=5;
twi_data_1[2]=1;
twi_data_1[3]=2;
twi_data_1[4]=3;
twi_data_1[5]=4;
twi_data_1[6]=5;
TWI_Start_Transceiver_With_Data(twi_data_1, 7);
break;
case SEND_ADC_DATA:
m_green(TOGGLE);
unsigned char twi_data_2[7];
twi_data_2[0]=SEND_ADC_DATA;
twi_data_2[1]=5;
twi_data_2[2]=(adc_value & 0x00FF);
twi_data_2[3]=((adc_value & 0xFF00)>>8);
twi_data_2[4]=(adc_value & 0x00FF);
twi_data_2[5]=((adc_value & 0xFF00)>>8);
twi_data_2[6]=0xAA;
TWI_Start_Transceiver_With_Data(twi_data_2, 7);
break;
case M_GREEN_ON:
m_green(ON);
break;
case M_BLUE_ON:
m_blue(ON);
break;
}
}
}
int main(void) {
m_disableJTAG();
m_clockdivide(2);
setup_pins();
if (debug_fire|| RF_debug) {setupUSB();}
setup_timer_1();
setup_timer_3();
m_bus_init();
m_rf_open(chan,RX_add,p_length);
int timer_3_cnt = 0;
//sei();
set_motors(0,0);
while (1){
if (check(TIFR3,OCF3A)){
set(TIFR3, OCF3A);
timer_3_cnt++;
if(fired){
since_fired++;
if (debug_fire){
m_usb_tx_string(" its been\t");
m_usb_tx_int(since_fired);
m_usb_tx_string(" milisec\n\r");
}
}
if (since_fired>10){
clear(PORTF,5);
since_fired=0;
fired = false;
if (debug_fire){m_usb_tx_string(" its been 100 sec\n\r");}
}
if ( fire && check(PINB,3)){
fire=false;
fired=true;
since_fired=0;
if (debug_fire){m_usb_tx_string(" portb 3 is high\n\r");}
}
// m_rf_open(chan,RX_add,p_length);
m_green(TOGGLE);
m_rf_init();
if ( timer_3_cnt == 10){
m_red(2);
timer_3_cnt=0;
m_rf_open(chan,RX_add,p_length);
}
}
if(new){ turretDrive();
if(RF_debug){ debug_rf(); }
if ((receive_buffer[0] == 1 || receive_buffer[1]==1) && !fire){FIRE();}
}
//TODO fill in timer code for the firing mechanism
}
}
virtual void
convert( norm_double val, NormRgb & nrgb ) override
{
if ( ! std::isfinite( val ) ) {
nrgb.fill(0.0);
}
nrgb[0] = m_red(val);
nrgb[1] = m_green(val);
nrgb[2] = m_blue(val);
}
void process_command(void)
{
if (twi_data.data_length > 255) // in the m_twi.h, the buffer size is defined as 255, you can change this value
{
twi_data.command = I2C_ERROR;
twi_data.data_length = 0;
return;
}
switch (twi_data.command)
{
case M_BLUE_TOGGLE:
m_blue(TOGGLE);
twi_data.command=M_BLUE_TOGGLE;
twi_data.data_length=5;
twi_data.data[0]=1;
twi_data.data[1]=2;
twi_data.data[2]=3;
twi_data.data[3]=4;
twi_data.data[4]=5;
break;
case M_GREEN_TOGGLE:
m_green(TOGGLE);
twi_data.command=M_GREEN_TOGGLE;
twi_data.data_length=4;
twi_data.data[0]=5;
twi_data.data[1]=6;
twi_data.data[2]=7;
twi_data.data[3]=8;
break;
case M_GREEN_ON:
m_green(ON);
break;
case M_BLUE_ON:
m_blue(ON);
break;
}
}
/* Update Targets, Gains, and Max Vals */
void bot_behavior_update()
{
if (has_puck())
{
positioning_LED(RED);
x_target = x_goal;
y_target = y_goal;
max_theta = M_PI/2;
theta_kd = 0.05;
theta_kp = 1.6;
max_duty_cycle = DUTY_CYCLE_PUCK;
m_green(OFF);
return;
}
if (!has_puck())
{
positioning_LED(RED);
float position_buffer[3];
get_position(position_buffer);
if((puck_dist>800) && (fabs(position_buffer[0]-x_goal)<(fabs(x_puck-x_goal))))
{
x_target = -x_goal;
y_target = y_goal;
} else {
x_target = x_puck;
y_target = y_puck;
if (!x_puck && !y_puck) {
positioning_LED(BLUE);
}
}
max_theta = M_PI;
theta_kd = 0;
theta_kp = 2.2;
max_duty_cycle = DUTY_CYCLE_SEEK;
m_green(ON);
return;
}
}
/**********************************************************************************
COMMAND EXECUTION
***********************************************************************************/
int comm(int commd)
{
if(commd)
{
//m_red(TOGGLE);
switch (commd){
case commandTest:
m_red(ON);
m_wait(500);
m_red(OFF);
m_wait(500);
break;
case PLAY:
m_green(TOGGLE);
break;
case GOALA:
OCR1B = 0;
OCR1C = 0;
break;
case GOALB:
OCR1B = 0;
OCR1C = 0;
break;
case PAUSE:
OCR1B = 0;
OCR1C = 0;
break;
case HALFTIME:
OCR1B = 0;
OCR1C = 0;
break;
case GAMEOVER:
OCR1B = 0;
OCR1C = 0;
break;
default:
OCR1B = 0;
OCR1C = 0;
commd = 0;
return 0;
break;
}
return 1;
}
return 0;
}
void gen_setup(void){
usb_conn_test = Matlab_pos_tracking || go_to_location_test || debug_com || debug_goto || debug_find_puck;
if (usb_conn_test ){
m_usb_init();
m_green(ON);
while (!m_usb_isconnected());
m_green(OFF);
}
m_bus_init();
m_wii_open();
// m_port_init(PORT_ADD);
setup_pins();
setup_puckfinding(); //Puck Finding Intialization
setup_timer_1();
setup_timer_3();
setup_ADC();
}
int main() {
m_red(ON);
m_clockdivide(0);
m_disableJTAG();
m_bus_init();
m_rf_open(1, 0xD1, 10);
m_red(OFF);
char buf[10] = {0x08, 10, 10, 0, 0, 0, 0, 0, 0, 0};
while(1) {
m_green(ON);
m_rf_send(0xDA, buf, 10);
buf[1] += 5;
if(buf[1] >= 220) buf[1] = 10;
m_green(OFF);
m_wait(500);
}
return 0;
}
int main(void){
m_clockdivide(0);
int data[9] = {0,0,0,0,0,0,0,0,0};
m_green(ON);
m_bus_init();
m_usb_init();
while(!m_usb_isconnected());
m_green(OFF);
m_imu_init(accel_scale, gyro_scale);
m_rf_open(CHANNEL, RXADDRESS, PACKET_LENGTH);
while(1) {
while ( !newPacket );
int i,a;
while (i=0; i<PACKET_LENGTH , i+=2 ){
a = *(int*)&packet[i*2+2];
m_usb_tx_int(a);
m_usb_tx_char('\t');
}
m_usb_tx_char('\n');
m_usb_tx_char('\r');
}
}
int main(void)
{
/* Confirm Power */
m_red(ON);
/* Initializations */
init();
usb_enable();
wireless_enable();
/* Confirm successful initialization(s) */
m_green(ON);
while (1)
{}
}
/************************************************************
Main Loop
************************************************************/
int main(void)
{
/* Confirm Power */
m_red(ON);
/* Initializations */
init(); // Set pins, clock speed, enable interrupts
enable_timer_one(); // Configure timer one
enable_timer_three(); // Configure timer three
enable_wireless(); // Configure wireless module
//enable_usb(); // Configure USB Communications
/* Confirm successful initialization(s) */
m_green(ON);
m_red(OFF);
/* Run */
while (1){}
}
int main(void)
{
m_green(ON);
sei();
set(DDRB,0);
set(DDRB,1);
set(DDRB,2);
set(DDRB,3);
//Timer initialization
clear(TCCR1B,CS12); //Set timer1 prescaler to /1
clear(TCCR1B,CS11);
set(TCCR1B,CS10);
clear(TCCR1B,WGM13); //Use timer mode 4 (up to OCR1A)
set(TCCR1B,WGM12);
clear(TCCR1A,WGM11);
clear(TCCR1A,WGM10);
clear(TCCR1A,COM1B1); //No change of B6
clear(TCCR1A,COM1B0);
OCR1A = CLOCK/PWM_FREQ;
OCR1B = ((float)OCR1A)*0.3;
OCR1C = ((float)OCR1A)*0.3;
set(TIMSK1,OCIE1A); // OCR1A interrupt vector
set(TIMSK1,OCIE1B); // OCR1B interrupt vector
set(TIMSK1,OCIE1C); // OCR1C interrupt vector
set(PORTB,0); // B0 Left motor enable
set(PORTB,2); // B2 Right motor enable
clear(PORTB,1);
clear(PORTB,3);
while (1)
{}
}
int main(void)
{
// unsigned int value;
/* insert your hardware initialization here */
m_usb_init();
while (!m_usb_isconnected());
for(;;) {
// if (m_usb_rx_available()) {
// value = m_usb_rx_char();
// m_usb_tx_uint(value);
// }
m_wait(500);
m_green(TOGGLE);
}
return 0; /* never reached */
}
void calibrate(int select){
long axbar = 0;
long aybar = 0;
long azbar = 0;
long gxbar = 0;
long gybar = 0;
long gzbar = 0;
int bar[6];
selectIMU(select);
for(i = 0; i < 100; i++){
m_wait(10);
m_green(TOGGLE);
toggle(PORTB,2);
/*
m_imu_accel(raw_data_buffer);
axbar = axbar + (long)raw_data_buffer[0];
aybar = aybar + (long)raw_data_buffer[1];
azbar = azbar + (long)raw_data_buffer[2];
*/
m_imu_gyro(raw_data_buffer);
gxbar = gxbar + (long)raw_data_buffer[0];
gybar = gybar + (long)raw_data_buffer[1];
gzbar = gzbar + (long)raw_data_buffer[2];
}
bar[0] = (int)axbar/i;
bar[1] = (int)aybar/i;
bar[2] = (int)azbar/i;
bar[3] = (int)gxbar/i;
bar[4] = (int)gybar/i;
bar[5] = (int)gzbar/i;
switch(select){
case 0:
for(i = 0; i < 6; i++){
bar0[i] = bar[i];
}
break;
case 1:
for(i = 0; i < 6; i++){
bar1[i] = bar[i];
}
break;
case 2:
for(i = 0; i < 6; i++){
bar2[i] = bar[i];
}
break;
case 3:
for(i = 0; i < 6; i++){
bar3[i] = bar[i];
}
break;
case 4:
for(i = 0; i < 6; i++){
bar4[i] = bar[i];
}
break;
case 5:
for(i = 0; i < 6; i++){
bar5[i] = bar[i];
}
break;
}
}
int main(void)
{
unsigned long runtime = 0;
m_clockdivide(0); // 16 MHz
OCR1B = 521; // initialize output compare register (interrupt every 521 cycles)
set(DDRB,6); // set B6 as output (output compare pin)
set(DDRB,4); // set B4 as output (green LED for data synchronization)
set(PORTB,4); // turn on green LED on B4
set(DDRF,0); // set F0 as output
set(DDRF,1); // set F1 as output
set(DDRB,1); // set B1 as output
m_usb_init();
while(!m_usb_isconnected()){
m_green(ON);
}
m_green(OFF);
set(DDRB,2);
while(!m_usb_rx_available()); // wait for runtime argument from Python script
m_wait(5);
while(m_usb_rx_available()){
runtime = runtime * 10 + (m_usb_rx_char() - '0'); // build number of seconds
}
runtime = runtime * 1000000; // convert to microseconds
clear(PORTF,0); // S0
clear(PORTF,1); // S1
clear(PORTB,1); // S2
m_wait(10);
if(!m_imu_init(0,1)){
m_red(ON); // RED LED turns on if there's a problem
m_usb_tx_string("IMU0 could not connect");
}
/*
set(PORTF,0);
m_wait(10);
if(!m_imu_init(0,1)){
m_red(ON); // RED LED turns on if there's a problem
m_usb_tx_string("IMU1 could not connect");
}
clear(PORTF,0);
set(PORTF,1);
m_wait(10);
if(!m_imu_init(0,1)){
m_red(ON); // RED LED turns on if there's a problem
m_usb_tx_string("IMU2 could not connect");
}
set(PORTF,0);
m_wait(10);
if(!m_imu_init(0,1)){
m_red(ON); // RED LED turns on if there's a problem
m_usb_tx_string("IMU3 could not connect");
}
set(PORTB,1);
clear(PORTF,0);
clear(PORTF,1);
m_wait(10);
if(!m_imu_init(0,1)){
m_red(ON); // RED LED turns on if there's a problem
m_usb_tx_string("IMU4 could not connect");
}
set(PORTF,0);
m_wait(10);
if(!m_imu_init(0,1)){
m_red(ON); // RED LED turns on if there's a problem
m_usb_tx_string("IMU5 could not connect");
}
*/
//calibrate(0);
//calibrate(1);
//calibrate(2);
//calibrate(3);
//calibrate(4);
//calibrate(5);
// Initialize Timer 1 output compare interrupt
set(TIMSK1,OCIE1B);
set(TCCR1B,CS12);
clear(TCCR1B,CS11);
set(TCCR1B,CS10);
clear(TCCR1B,WGM13);
set(TCCR1B,WGM12);
clear(TCCR1A,WGM11);
clear(TCCR1A,WGM10);
clear(TCCR1A,COM1B1);
set(TCCR1A,COM1B0);
// Initialize Timer 1 overflow interrupt
set(TIMSK1,TOIE1);
// Initialize Timer 3 overflow interrupt
overflow = 0;
set(TIMSK3,TOIE3);
clear(TCCR3B,CS32);
clear(TCCR3B,CS31);
set(TCCR3B,CS30);
sei(); // enable global interrupts
clear(PORTB,4); // turn off green LED --> signal to camera
while(overflow * 4096 < runtime){
m_green(TOGGLE);
}
m_usb_tx_string(" ");
m_green(OFF);
//cli();
while(1);
}
int main(void)
{
m_disableJTAG();
m_red(ON);
m_clockdivide(0); // clock speed 16 MHz
sei(); // enable global interrups
// initialize m_bus communications
// start wireless communications
// and open wii camera
m_bus_init();
m_wii_open();
m_usb_init();
m_rf_open(CHANNEL, RXADDRESS, PACKET_LENGTH);
setup(); // motor setup
ADC_setup();
int position[3]; // array for robot position
int x_curr;
int y_curr;
int theta_curr;
int i;
int a;
int x_pos[11];
int y_pos[11];
int theta[11];
// main LOOP
while(1)
{
// for (i=0; i < 11; i++)
// {
// current_location(position);
// x_pos[i] = position[0];
// y_pos[i] = position[1];
// theta[i] = position[2];
// }
// // calculate positions
// filter(x_pos,y_pos,theta);
// x_curr = x_pos[11];
// y_curr = y_pos[11];
// theta_curr = theta[11];
// Read in values
// ADC0();
// int D6 = ADC;
//
// ADC1();
// int F1 = ADC;
//
// ADC4();
// int F4 = ADC;
//
// ADC5();
// int F5 = ADC;
//
// ADC6();
// int F6 = ADC;
//
// ADC7();
// int F7 = ADC;
//
// ADC8();
// int D7 = ADC;
//
follow_puck();
m_green(TOGGLE);
// // usb prinitng
// m_usb_tx_string("D6: ");
// m_usb_tx_int(D6);
// m_usb_tx_string(" ");
// m_usb_tx_string("F1: ");
// m_usb_tx_int(F1);
// m_usb_tx_string(" ");
// m_usb_tx_string("F4: ");
// m_usb_tx_int(F4);
// m_usb_tx_string(" ");
// m_usb_tx_string("F5: ");
// m_usb_tx_int(F5);
// m_usb_tx_string(" ");
// m_usb_tx_string("F6: ");
// m_usb_tx_int(F6);
// m_usb_tx_string(" ");
// m_usb_tx_string("F7: ");
// m_usb_tx_int(F7);
// m_usb_tx_string(" ");
// m_usb_tx_string("D7: ");
// m_usb_tx_int(D7);
// m_usb_tx_string(" ");
// m_usb_tx_string("\n");
a = state(buffer1);
a = 2;
// COMM TEST
if (a == 1)
{
// ADD CODE
}
// PLAY
if (a == 2) {
}
// PAUSE, HALF TIME, GAME OVER
if (a == 5 || a == 6 || a == 7)
{
// STOP ROBOT
turn_robot(0,0,0,0);
m_red(OFF);
}
else
{}
}
}
void setupUSB(){
m_usb_init();
m_green(ON);
while (!m_usb_isconnected()){m_wait(1);}
m_green(OFF);
}
void position_demo()
{
m_usb_init();
set(DDRD,5);
//init mWii
char wii_status = m_wii_open();
if(wii_status){
m_green(ON);
set(PORTD,5);
}else
m_red(ON);
while(!m_usb_isconnected())
{
m_green(ON);
}
unsigned int blobs[] = {541,381,0,503,344,0,524,304,0,599,347,0};
int blob_status = 0;
while(1)
{
double x;
double y;
double theta;
if(m_usb_isconnected()){
blob_status = m_wii_read(blobs);
if(blob_status){
set_position(1024/2,768/2);
get_position(blobs,&x,&y,&theta);
/*
m_usb_tx_string("x: ");
m_usb_tx_int((int)(x * 100));
m_usb_tx_string("\n");
m_usb_tx_string("y: ");
m_usb_tx_int((int)(y * 100));
m_usb_tx_string("\n");
m_usb_tx_string("theta: ");
m_usb_tx_int((int)(theta * 100));
m_usb_tx_string("\n");
m_usb_tx_string("blob1x: ");
m_usb_tx_int((int) blobs[0]);
m_usb_tx_string("blob1y: ");
m_usb_tx_int((int) blobs[1]);
m_usb_tx_string("\n");
m_usb_tx_string("blob2x: ");
m_usb_tx_int((int) blobs[3]);
m_usb_tx_string("blob2y: ");
m_usb_tx_int((int) blobs[4]);
m_usb_tx_string("\n");
m_usb_tx_string("blob3x: ");
m_usb_tx_int((int) blobs[6]);
m_usb_tx_string("blob3y: ");
m_usb_tx_int((int) blobs[7]);
m_usb_tx_string("\n");
m_usb_tx_string("blob4x: ");
m_usb_tx_int((int) blobs[9]);
m_usb_tx_string("blob4y: ");
m_usb_tx_int((int) blobs[10]);
m_usb_tx_string("\n");
*/
m_usb_tx_int((int)(x * 100));
m_usb_tx_int((int)(y * 100));
m_usb_tx_int((int)(theta * 100));
m_usb_tx_int((int) blobs[0]);
m_usb_tx_int((int) blobs[1]);
m_usb_tx_int((int) blobs[3]);
m_usb_tx_int((int) blobs[4]);
m_usb_tx_int((int) blobs[6]);
m_usb_tx_int((int) blobs[7]);
m_usb_tx_int((int) blobs[9]);
m_usb_tx_int((int) blobs[10]);
m_wait(1000);
m_red(TOGGLE);
}
}
}
}
void abort(void) {cli();while(1){m_red(ON);m_green(OFF);}}
int main(void)
{
//original data
int Data[9]={0};
int RealinputACC = 0;
int RealinputGyr = 0;
//variables of flags
int switchNum = 0;
int paraDeter = 0;
//temporary variables for test
int tmp=0;
int getValue=0;
//variables for PID control
int output;
int angleHis1=0,angleHis2=0;
int inputK=0,inputI=0,inputD=0;
int Output=0;
int OutputHis1=2000;
int OutputHis2=2000;
int KpIni=KPINI,KiIni=KIINI,KdIni=KDINI;
float Kp = 1;
float Ki = 0;
float Kd = 0.5;
int AngleCali = 0;
int AngleActual=0;
// Parameters for speed control
int speed = 0;
// PID variables for speed controller
int speedHis1 = 0, speedHis2 = 0;
int speedI = 0, speedD = 0;
int outputSpeed =0;
systemInitial();
// send back comfirm information to prove that wireless has been connected
char haha[5]="Hello";
wireless_string(haha,5);
wireless_char('\n');
clear(DDRB,0);
clear(PORTB,0);
// using wireless to change parameters of PID controller, and if DIP switch 1 is ON, this can be skipped
if (!check(PINB,0))
{
while(1)
{
//m_green(ON);
int success;
success = m_rf_read(buffer, packLength);
if ((success==1)&&(buffer[0]>=45))
{
m_green(ON);
ConvertFinishFlag=1;
}
else{
m_green(OFF);
}
if (ConvertFinishFlag==1)
{
if (switchNum==0)
{
// send back current value of parameter and determine which one should be changed
wireless_string(buffer,packLength);
wireless_char(':');
if ((buffer[0]=='k')&&(buffer[1]=='p'))
{
paraDeter = 1;
wireless_int(KpIni);
}
if ((buffer[0]=='k')&&(buffer[1]=='i'))
{
paraDeter = 2;
wireless_int(KiIni);
}
if ((buffer[0]=='k')&&(buffer[1]=='d'))
{
paraDeter = 3;
wireless_int(KdIni);
}
m_wait(50);
wireless_char('\n');
switchNum++;
}else{
// change the parameter, and if the input is not a value, it will send back "SayAgain" and wait untill the input is a value
getValue = atoi(buffer);
if ((getValue==0)&&(buffer[0]!='0'))
{
char Sorry1[8] = "SayAgain";
wireless_string(Sorry1,8);
m_wait(50);
wireless_char('\n');
m_wait(50);
switchNum=1;
}else{
wireless_string(buffer,packLength);
m_wait(50);
wireless_char('\n');
m_wait(50);
switchNum=0;
switch (paraDeter)
{
case 1:KpIni=getValue;break;
case 2:KiIni=getValue;break;
case 3:KdIni=getValue;break;
}
}
}
// when the input is "start", the robot will start
if ((buffer[0]=='s')&&(buffer[1]=='t')&&(buffer[2]=='a')&&(buffer[3]=='r')&&(buffer[4]=='t'))
{
break;
}
ConvertFinishFlag=0;
memset(buffer,0,packLength);
m_wait(100);
}
}
}
m_green(OFF);
// Initializing IMU
while(!m_imu_init(1,0));
int AngleHis=0;
// calibrate the balance angle value, the code here is trying to find out offset of the angle
for (int numpoint=0;numpoint<499;numpoint++)
{
if(m_imu_raw(Data))
{
RealinputACC = ACCPART*(Data[1]-ACOFFSET);
RealinputGyr = GYRPART*(GYOFFSET-Data[3]);
Kalman_Filter(RealinputACC,RealinputGyr);
}
AngleHis+=angle;
}
AngleCali = AngleHis/500;
// Enable timer interrupt and global interrupt
set(TIMSK3 , OCIE3A);
// Initializing the pin change interrupt which will capture the phase of the signal input of the encoder
set(PCMSK0 , PCINT4);
set(PCICR , PCIE0);
clear(DDRB,4);
clear(DDRB,5);
sei();
// Initializing all the parameters will be used in the PID control of the speed ( or we could say 'displacement')
Kp = (float)KpIni/1000;
Ki = (float)KiIni/1000;
Kd = (float)KdIni/1000;
float Kps, Kds, Kis;
int OutputSpeedActual = 0;
Kps = 4;
Kds = 5;
Kis = 0.5;
while(1)
{
if (Timer3Flag==1) //Here is the control loop, all the data sample process and output should be here
{
cli(); //Try to reduce the possibility of changing the data read from IIC, cause multiple device will use the same line and there also different interrupt in the program
// read data from IMU
if(m_imu_raw(Data))
{
m_red(TOGGLE);
}
// make the input value get rid of the offset
RealinputACC = ACCPART*(Data[1]-ACOFFSET); //The acceleration input without offset
RealinputGyr = GYRPART*(Data[3]-GYOFFSET); //The anglar velocity input without offset
Kalman_Filter(RealinputACC,RealinputGyr); //Using the Kalman Filter to get the reliable output of the angle
// read the changed parameter, because the wireless cannot send float, so change it to float here
Kp = (float)KpIni/1000;
Ki = (float)KiIni/1000;
Kd = (float)KdIni/1000;
// calibrate the angle value by using the balance offset of the angle
AngleActual = angle-AngleCali;
inputK = AngleActual-angleHis1;
inputI +=AngleActual;
// PID controller, which you will find that Ki always be zero
Output = Kp*AngleActual + Ki*inputI + Kd*inputK;
// dead region, which means the value in this region won't give the wheels a speed, so remove it from output to make the output speed linear with the input angle
if (AngleActual>0)
{
OutputHis1 =1823-Output;
}
if (AngleActual<0)
{
OutputHis1 =2193-Output;
}
if (AngleActual==0)
{
if (angleHis1>0)
{
OutputHis1 =1823;
}
if (angleHis1<0)
{
OutputHis1 =2193;
}
if (angleHis1=0)
{
OutputHis1 =OutputHis1;
}
}
if (AngleActual>0)
{
OutputHis2 =1857-Output;
}
if (AngleActual<0)
{
OutputHis2 =2148-Output;
}
if (AngleActual==0)
{
if (angleHis1>0)
{
OutputHis2 =1857;
}
if (angleHis1<0)
{
OutputHis2 =2148;
}
if (angleHis1=0)
{
OutputHis2 =OutputHis2;
}
}
// Try to check whether the output value in the limitation
if (OutputHis1>4000) //Detect the limitation of the output value
{
OutputHis1=4000;
}else{
if (OutputHis1<3)
{
OutputHis1=3;
}
}
if (OutputHis2>4000) //Detect the limitation of the output value
{
OutputHis2=4000;
}else{
if (OutputHis2<3)
{
OutputHis2=3;
}
}
// set the output duty cycle
OCR1B = OutputHis1; // for B6
OCR1C = OutputHis2; // for B7
angleHis2 = angleHis1; //record value of the angle which will be used in PID controller(differential)
angleHis1 = AngleActual;//record value of the angle which will be used in PID controller(differential)
Timer3Flag=0;
}
if (Timer3Flag2==INTERRUPT1S) //this condition used to send the wireless data or usb data, cause the frequency here is below 10Hz
{
cli(); // same concern with above
speed += numPulse; //red the the value of holes (net value, which means one direction is positive and the other is negative, here is just a summation)
speedI += speed;
speedD = speed - speedHis1;
// PID controller of the speed(displacement actually)
outputSpeed = Kps*speed + Kis*speedI + Kds*speedD;
//if (speed>0)
//{
//OutputSpeedActual =1800-outputSpeed;
//}
//if (speed<0)
//{
//OutputSpeedActual =2200-outputSpeed;
//}
//if (speed==0)
//{
//if (speed>0)
//{
//OutputSpeedActual =1800;
//}
//if (speed<0)
//{
//OutputSpeedActual =2200;
//}
//if (speed=0)
//{
//OutputSpeedActual =OutputSpeedActual;
//}
//}
// For stability concern, I decide to limit this value, which means the maximum change of duty cycle is 500/4000 = 12.5%
if (outputSpeed>500)
{
outputSpeed=500;
}
if (outputSpeed<-500)
{
outputSpeed=-500;
}
OCR1B = OutputHis1+outputSpeed;
OCR1C = OutputHis2+outputSpeed;
speedHis2 = speedHis1;
speedHis1 = speed;
// all the wireless receiving and sending codes should be wrote here
wireless_int(RealinputACC); // send back value from IMU of acceleration of Y
wireless_char('\t');
wireless_int(RealinputGyr); // send back value from IMU of angular speed around X
wireless_char('\t');
wireless_int(AngleActual); // send back the current angle value
wireless_char('\n');
m_green(TOGGLE);
Timer3Flag2=0;
// clear the counter
numPulse = 0;
sei();
}
//if (Timer3Flag3==INTERRUPT10MS)
//{
//}
}
}
int main(void)
{
robockey_init();
while(1)
{
comm(command);
mode = check_mode();
if(wii_flag && command == PLAY)
{
location_flag = localization(blobs, calibration, location);
wii_flag = 0;
}
switch(mode)
{
case 0: // normal mode(GOAL A), need command PLAY TO START
if(switch_flag)
{
goal[0] = -125;
goal[1] = 0;
goal[2] = 0;
if(command == HALFTIME)
{
goal[0] = 125;
switch_flag = 0;
}
}
if(command == PLAY)
{
//if(!found)
//{
//found = find(adc1, adc2, &count);
//set(TIMSK0,OCIE0A); // Enable timer0 interrupt
//}
if(find(adc1, adc2, &count))
{
m_green(TOGGLE);
m_red(OFF);
move(location, goal);
}
}
break;
case 1: // normal mode(GOAL B), need command PLAY TO START
if(switch_flag)
{
goal[0] = 125;
goal[1] = 0;
goal[2] = 0;
if(command == HALFTIME)
{
goal[0] = -125;
switch_flag = 0;
}
}
if(command == PLAY)
{
if(find(adc1, adc2, &count))
{
m_green(TOGGLE);
m_red(OFF);
move(location, goal);
}
}
break;
case 2: // calibrate the center of the rink
m_green(ON);
m_red(ON);
if(wii_flag)
{
float calibrationDefault[2] = {0, 0};
float calibrationResult[3] = {0, 0, 0};
if(localization(blobs, calibrationDefault, calibrationResult))
{
//m_green(ON);
//m_red(ON);
calibration[0] = calibrationResult[0];
calibration[1] = calibrationResult[1];
}
}
break;
case 3: // test mode(GOAL A), always play while the power is on.
m_green(TOGGLE);
m_red(OFF);
if(find(adc1, adc2, &count))
{
if(wii_flag)
{
//if(!found)
//{
//found = find(adc1, adc2, &count);
//set(TIMSK0,OCIE0A); // Enable interrupt
//}
if(localization(blobs, calibration, location))
{
float goal[3] = {-115, 0, 0};
move(location, goal);
}
}
}
break;
case 4: // test mode(GOAL B), always play while the power is on.
m_red(TOGGLE);
m_green(OFF);
if(find(adc1, adc2, &count))
{
if(wii_flag)
{
if(localization(blobs, calibration, location))
{
float goal[3] = {115, 0, 0};
move(location, goal);
}
}
}
break;
case 5:
m_green(ON);
m_red(ON);
OCR1B = OCR1A;
OCR1C = 0.98*OCR1A;
break;
case 6:
qualify_test(&command, blobs, location, &orientation_flag, &west_flag, &east_flag, calibration);
break;
case 7:
OCR1B = OCR1A;
OCR1C = 0;
m_wait(3200);
OCR1B = OCR1A/2;
OCR1C = OCR1A/2;
m_wait(32000);
break;
default:
OCR1B = 0.5*OCR1A;
OCR1C = 0.5*OCR1A;
m_green(OFF);
m_red(OFF);
}
m_usb_tx_int((int)(location[0]));
m_usb_tx_char('\t');
m_usb_tx_int((int)(location[1]));
m_usb_tx_char('\t');
m_usb_tx_int((int)(adc1[0]));
m_usb_tx_char('\t');
m_usb_tx_int((int)(adc2[0]));
m_usb_tx_char('\t');
m_usb_tx_int((int)(check(PIND, 7)));
m_usb_tx_char('\t');
m_usb_tx_char('\r');
if(!check(ADCSRA, ADIF)) set(ADCSRA,ADSC);
}
}
