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 comm_test() {
set(PORTD,5);
m_wait(500);
clear(PORTD,5);
m_wait(500);
state = 0;
}
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 testTeamLEDPins()
{
// Set up Team LEDs on MAX7219
sendSPI(0x0200 + 0x04); m_wait(TEST_TEAM_LED_TIME_MS);
sendSPI(0x0200 + 0x08); m_wait(TEST_TEAM_LED_TIME_MS);
sendSPI(0x0200 + 0x00);
//m_set(RED_LED); m_wait(TEST_TEAM_LED_TIME_MS);
//m_set(BLUE_LED); m_wait(TEST_TEAM_LED_TIME_MS);
}
void commtestf(void)
{ clearDisplay();
dispH();
m_wait(100);
clearDisplay();
m_wait(100);
dispE();
m_wait(100);
clearDisplay();
m_wait(100);
dispL();
m_wait(100);
clearDisplay();
m_wait(100);
dispL();
m_wait(100);
clearDisplay();
m_wait(100);
disp0();
m_wait(100);
clearDisplay();
state = 0x00;
}
/**********************************************************************************
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 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;
}
}
}
static void shjump(int sig)
{
char *sh;
int pid;
int status;
int f;
(void)sig;
sh = getenv("SHELL");
if (sh == NULL) {
werror(_("SHELL variable not set"));
return;
}
if ((pid = fork()) == -1) {
werror(_("Out of memory: could not fork()"));
return;
}
if (pid != 0)
mc_wleave();
if (pid == 0) {
for (f = 1; f < _NSIG; f++)
signal(f, SIG_DFL);
for (f = 3; f < 20; f++)
close(f);
fputs(_("Shelled out. Type \"exit\" to return.\n"), stdout);
execl(sh, sh, NULL);
exit(1);
}
m_wait(&status);
mc_wreturn();
if (use_status)
show_status();
}
int main(void)
{
m_clockdivide(0);
unsigned char accel_scale = 0;
unsigned char gyro_scale = 0;
int data[9] = {0,0,0,0,0,0,0,0,0};
m_usb_init();
m_imu_init(accel_scale, gyro_scale);
while(1)
{
m_wait(10);
m_imu_raw(data);
m_usb_tx_int(data[0]);
m_usb_tx_char('\t');
m_usb_tx_int(data[1]);
m_usb_tx_char('\t');
m_usb_tx_int(data[2]);
m_usb_tx_char('\t');
m_usb_tx_int(data[3]);
m_usb_tx_char('\t');
m_usb_tx_int(data[4]);
m_usb_tx_char('\t');
m_usb_tx_int(data[5]);
m_usb_tx_char('\n');
m_usb_tx_char('\r');
}
}
void shoot() {
//m_wait(1000);
m_red(ON);
set(PORTB,0);
set(DDRB,7);
set(PORTB,7);
m_wait(200);
clear(PORTB,7);
m_red(OFF);
clear(DDRB,7);
clear(PORTB,0);
}
/*
* Run kermit. Used to do this in the main window, but newer
* versions of kermit are too intelligent and just want a tty
* for themselves or they won't function ok. Shame.
*/
void kermit(void)
{
int status, pid, n;
char * translated_cmdline;
char *kermit_path = P_KERMIT;
if (!kermit_path || !*kermit_path) {
werror("No kermit path defined!");
return;
}
/* Clear screen, set keyboard modes etc. */
mc_wleave();
switch (pid = fork()) {
case -1:
mc_wreturn();
werror(_("Out of memory: could not fork()"));
return;
case 0: /* Child */
close(portfd);
/* Remove lockfile */
lockfile_remove();
for (n = 0; n < _NSIG; n++)
signal(n, SIG_DFL);
translated_cmdline = translate(P_KERMIT);
if (translated_cmdline != NULL) {
fastexec(translated_cmdline);
free(translated_cmdline);
}
exit(1);
default: /* Parent */
break;
}
m_wait(&status);
/* Restore screen and keyboard modes */
mc_wreturn();
/* Re-create lockfile */
lockfile_create();
m_flush(portfd);
port_init();
}
void state_detangle()
{
m_red(ON);
set_left(-90);
set_right(-50);
m_wait(2000);
m_red(OFF);
set_left(0);
set_right(0);
char transition = wireless_buffer[0];
switch(transition)
{
case 0xA1:
state_play();
return;
}
}
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 */
}
int
main(int argc, char *argv[])
{
int opt;
int hasEnded = 0;
int i;
for (i=0; i < MAX_SEM; i++)
sem_array[i] = SEM_FAILED;
while ( !hasEnded )
{
printf("Escriba la opcion deseada\n1 - OPEN\t2 - WAIT\t3 - POST\t4 - CLOSE\n");
printf( "5 - UNLINK\t6 - GETVALUE\t7 - TRYWAIT\t8 - INIT\n");
printf( "9 - DESTROY\t10 - FREE SPACE\n");
if ( get_num(&opt) == -1 )
{
hasEnded = 1;
continue;
}
putchar('\n');
switch (opt)
{
case 1: m_open(); break;
case 2: m_wait(); break;
case 3: m_post(); break;
case 4: m_close(); break;
case 5: m_unlink(); break;
case 6: m_getvalue(); break;
case 7: m_trywait(); break;
case 8: m_sem_init(); break;
case 9: m_destroy(); break;
case 10: free_space(); break;
default: printf("Opcion invalida\n");
}
}
}
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)
{
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);
}
}
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);
}
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;
}
}
void setupUSB(){
m_usb_init();
m_green(ON);
while (!m_usb_isconnected()){m_wait(1);}
m_green(OFF);
}
int main(void)
{
m_clockdivide(0);
m_bus_init();
m_usb_init();
m_rf_open(CHANNEL, RXADD, PACKET_LENGTH); // Configure the RF module to listen on CHANNEL for RXADD
sei(); //Enable global interrupt
unsigned int star[12]={0};
int i, j, k =0;
int maxD, minD, dist, p, q, r, s=0; // p.q.r.s -> co-ordinates between which the midpoint is to be found(x2, x4)[p.q.r.s=x2.y2.x4.y4]
int cx, cy, cxx, cyy, cX, cY=0;
int count=0;
int pmax, qmax, rmax, smax, pmin, qmin, rmin, smin, pp, qq, rr, ss=0;
float theta=0;
float to_angle, to_angle_1, to_angle_2, to_angle_c=0;
float temp1=0;
float LESS2=0;
float temp_theta=0;
int trans_c,trans_fl,trans_fr,trans_bl,trans_br=0;
int puck_in_hand, puck_in_line=0;
set(DDRD,7); // Set as Comm test Led output
set(DDRD,6); // Set as PAUSE Led output
/*
//ADC initialization
clear(ADMUX,REFS1); //Set voltage reference for ADC as Vcc
set(ADMUX,REFS0);
set(ADCSRA,ADPS2); //Prescale ADC to 250 Khz
set(ADCSRA,ADPS1);
clear(ADCSRA,ADPS0);
set(DIDR0,ADC1D); //Disabling Digital inputs for ADC pins
set(DIDR0,ADC4D);
set(DIDR0,ADC5D);
set(DIDR0,ADC6D);
set(DIDR0,ADC7D);
*/
while (m_wii_open()!=1){}
//Timer Initialization
set(DDRB, 6); // Set (Reg B. Bit 6) as timer output. PWM 1
set(DDRB, 7); // Set (Reg B. Bit 7) as timer output. PWM 2
set(TCCR1B, WGM13); // Set Timer 1 Mode 15 (UP to OCR1A, PWM mode)
set(TCCR1B, WGM12);
set(TCCR1A, WGM11);
set(TCCR1A, WGM10);
set(TCCR1A, COM1C1); // Clear at OCR1B. Set at rollover |Timer B.6
clear(TCCR1A, COM1C0);
set(TCCR1A, COM1B1); // Clear at OCR1C. Set at rollover |Timer B.7
clear(TCCR1A, COM1B0);
OCR1A=1600; //1Khz Motor
OCR1B=0;
OCR1C=0;
clear(TCCR1B, CS12); // Initialize timer & Prescale /1 (Start Timer)
clear(TCCR1B, CS11);
set(TCCR1B, CS10);
while(1)
{
while(!play) //PAUSE
{
m_green(OFF);
set(PORTD, 6); //Switch on RED LED
OCR1B=0; //Stop Motor
OCR1C=0; //Stop Motor
clear(PORTB,6); //Stop Motor
clear(PORTB,7); //Stop Motor
}
/*
//Trans_C
clear(ADCSRA,ADEN); // Disable ADC system to change MUX bit
clear(ADCSRB,MUX5); //Set MUX bit to F1
clear(ADMUX,MUX2);
clear(ADMUX,MUX1);
set(ADMUX,MUX0);
set(ADCSRA,ADEN); //Enable the system
set(ADCSRA,ADSC); //Start the conversion
while(!check(ADCSRA,ADIF)){} //ADC conversion ongoing
set(ADCSRA,ADIF);// Clear the flag
trans_c=ADC;
//Trans_F_LEFT
clear(ADCSRA,ADEN); // Disable ADC system to change MUX bit
clear(ADCSRB,MUX5); //set MUX bit to F4
set(ADMUX,MUX2);
clear(ADMUX,MUX1);
clear(ADMUX,MUX0);
set(ADCSRA,ADEN); //Enable the system
set(ADCSRA,ADSC); //Start the conversion
while(!check(ADCSRA,ADIF)){} //ADC conversion ongoing
set(ADCSRA,ADIF);// Clear the flag
trans_fl=ADC;
//TRANS_F_RIGHT
clear(ADCSRA,ADEN); // Disable ADC system to change MUX bit
clear(ADCSRB,MUX5); //Set MUX bit to F5
set(ADMUX,MUX2);
clear(ADMUX,MUX1);
set(ADMUX,MUX0);
set(ADCSRA,ADEN); //Enable the system
set(ADCSRA,ADSC); //Start the conversion
while(!check(ADCSRA,ADIF)){} //ADC conversion ongoing
set(ADCSRA,ADIF);// Clear the flag
trans_fr=ADC;
//TRANS_B_LEFT
clear(ADCSRA,ADEN); // Disable ADC system to change MUX bit
clear(ADCSRB,MUX5); //Set MUX bit to F6
set(ADMUX,MUX2);
set(ADMUX,MUX1);
clear(ADMUX,MUX0);
set(ADCSRA,ADEN); //Enable the system
set(ADCSRA,ADSC); //Start the conversion
while(!check(ADCSRA,ADIF)){} //ADC conversion ongoing
set(ADCSRA,ADIF);// Clear the flag
trans_bl=ADC;
//TRANS_B_RIGHT
clear(ADCSRA,ADEN); // Disable ADC system to change MUX bit
clear(ADCSRB,MUX5); //set MUX bit to F7
set(ADMUX,MUX2);
set(ADMUX,MUX1);
set(ADMUX,MUX0);
set(ADCSRA,ADEN); //Enable the system
set(ADCSRA,ADSC); //Start the conversion
while(!check(ADCSRA,ADIF)){} //ADC conversion ongoing
set(ADCSRA,ADIF);// Clear the flag
trans_br=ADC;
*/
clear(PORTD,6); //Switch OFF RED LED
cli();
m_wii_read(star);
sei();
count=0;
//Localization of bot w.r.t constellation
//Reading no. of detectable stars
for(k=0;k<=3;++k)
{
if((int)star[3*k]==1023 && (int)star[3*k+1]==1023)
{count++;} //Count = No. of stars lost
}
if(count<=1) //Enter this only if <= 1 star has been lost.
{
m_red(OFF);
//Finding max and min distance
pmax=0; qmax=3; rmax=1; smax=4; //p.r=(x1. y1)
pmin=0; qmin=3; rmin=1; smin=4; //q.s=(x2. y2)
maxD=pow(((int)star[pmax]-(int)star[qmax]), 2) + pow(((int)star[rmax]-(int)star[smax]), 2);
minD=maxD;
for (i=0; i<3; i++)
{
for(j=i+1; j<=3; j++)
{
dist=pow(((int)star[i*3]-(int)star[j*3]), 2) + pow(((int)star[i*3+1]-(int)star[j*3+1]), 2);
if(dist>maxD)
{
maxD=dist;
pmax=i*3; qmax=j*3; rmax=pmax+1; smax=qmax+1;
}
if(dist<minD)
{
minD=dist;
pmin=i*3; qmin=j*3; rmin=pmin+1; smin=qmin+1 ;
}
} //for j
}//for i
//Check for intersection between the max & min points to find the top x.y
if((int)star[pmax]==(int)star[pmin] || (int)star[pmax]==(int)star[qmin])
{
//m_red(OFF);
p=pmax; r=rmax; q=qmax; s=smax;
pp=p; rr=r; qq=q; ss=s;
}
else if ((int)star[qmax]==(int)star[pmin] || (int)star[qmax]==(int)star[qmin])
{
//m_red(OFF);
p=qmax; r=smax; q=pmax; s=rmax;
pp=p; rr=r; qq=q; ss=s;
}
else //Retain the previous co-ordinates if it can't find intersection
{
p=pp; q=qq; r=rr; s=ss;
//m_red(ON);
}
//Center Point
cx=((int)star[p]+(int)star[q])/2-512;
cy=((int)star[r]+(int)star[s])/2-384;
//Theta
theta=3.14/2-atan2((int)star[s]-(int)star[r], (int)star[q]-(int)star[p]);
cxx=1*(cx*cos(theta) - cy*sin(theta));
cyy=1*(cx*sin(theta) + cy*cos(theta));
//Center offset
cxx=cxx-65-8;
cyy=cyy+25-105;
//Center in (cm)
cX=-cxx/4;
cY=cyy/4;
m_usb_tx_string(" Theta_old: ");
m_usb_tx_int(theta*57.6);
//
if(theta*57.6>=180)
{
theta=theta-2*3.14;
}
/* if(theta*57.6<-180) // Why is this there?
{ theta=2*3.14-theta;
}
*/
theta=-theta;
/*
//Puck detection
//trans_c|trans_fl|trans_fr|trans_bl|trans_br|
while(!puck_in_hand)
{
if(trans_c>trans_fl && trans_c>trans_fr && trans_c>trans_bl && trans_c>trans_br) //Puck found! Go straight!
{
puck_in_line=1;
temp1=MORE*OCR1A; //Straight!
OCR1B=temp1;
temp1=MORE*OCR1A;
OCR1C=temp1;
}
else if( (trans_fr > trans_fl && trans_fr > trans_bl) || (trans_br >trans_fl && trans_br > trans_bl) ) //If right p_t> left p_t, Rotate Clockwise
{
temp1=LESS*OCR1A; //Clockwise
OCR1B=temp1;
temp1=MORE*OCR1A;
OCR1C=temp1;
}
else //Rotate anti-clockwise
{
temp1=MORE*OCR1A;
OCR1B=temp1;
temp1=LESS*OCR1A;
OCR1C=temp1;
}
}
}*/
//Goal detection (Activated once puck is detected <puck_in_hand>)
to_angle_c=atan2(((goal_1+goal_2)/2-cY),(115-cX)); //Angle between the goal and current position
to_angle_1=atan2((goal_1-cY),(115-cX)); //Angle between goal_1 (+y) and current position
to_angle_2=atan2((goal_2-cY),(115-cX)); //Angle between goal_2 (-y) and current position
if ( (to_angle_1*57.6>0 && to_angle_2*57.6>0) || (to_angle_1*57.6<0 && to_angle_2*57.6<0) ) //If both the angles are + or - (Out of scope of goal)
{
to_angle= (abs(to_angle_1*57.6) < abs(to_angle_2*57.6) ) ? to_angle_1 : to_angle_2; //to_angle = whichever is lesser
}
else //Opposite sign (Within scope of goal)
{
to_angle=atan2(0,(115-cX)); //Go straight
}
//Difference
m_usb_tx_string(" Theta-to_angle: ");
m_usb_tx_int((theta*57.6)-(to_angle*57.6));
// temp_theta=(abs(theta*57.6)-abs(to_angle*57.6);
// LESS2=( (MORE-LESS)/(10-temp_theta) )*(temp_theta-10) + MORE;
if (theta*57.6<=to_angle*57.6+10 && theta*57.6>=to_angle*57.6-10) // Pointing to goal. Go straight.
{
temp1=MORE*OCR1A;
OCR1B=temp1;
temp1=MORE*OCR1A;
OCR1C=temp1;
m_green(ON);
m_usb_tx_string(" ||STRAIGHT||");
}
else if( theta*57.6<(to_angle*57.6-10) ) //Rotate anti-clockwise
{
temp1=LESS*OCR1A;
OCR1B=temp1;
temp1=MORE*OCR1A;
OCR1C=temp1;
m_green(OFF);
m_usb_tx_string(" ||ANTI-CLOCKWISE||");
}
else if ( theta*57.6>(to_angle*57.6+10) ) //Rotate clockwise
{
temp1=MORE*OCR1A;
OCR1B=temp1;
temp1=LESS*OCR1A;
OCR1C=temp1;
m_green(OFF);
m_usb_tx_string(" ||CLOCKWISE||");
}
set(PORTB,6);
set(PORTB,7);
}//end if (<= 1 star has been lost.)
else //If more than 1 star has been lost:
{
m_red(ON);
OCR1B=0; //Stop the motor
OCR1C=0;
clear(PORTB,6);
clear(PORTB,7);
p=-1; q=-1; r=-1; s=-1;
cxx=-1; cyy=-1;
theta=0;
}
/*
m_usb_tx_string("X1: ");
m_usb_tx_uint(star[0]);
m_usb_tx_string(" Y1: ");
m_usb_tx_uint(star[1]);
m_usb_tx_string(" X2: ");
m_usb_tx_uint(star[3]);
m_usb_tx_string(" Y2: ");
m_usb_tx_uint(star[4]);
m_usb_tx_string(" X3: ");
m_usb_tx_uint(star[6]);
m_usb_tx_string(" Y3: ");
m_usb_tx_uint(star[7]);
m_usb_tx_string(" X4: ");
m_usb_tx_uint(star[9]);
m_usb_tx_string(" Y4: ");
m_usb_tx_uint(star[10]);
m_usb_tx_string(" P: ");
m_usb_tx_int(p);
m_usb_tx_string(" R: ");
m_usb_tx_int(r);
m_usb_tx_string(" Q: ");
m_usb_tx_int(q);
m_usb_tx_string(" S: ");
m_usb_tx_int(s);
*/
m_usb_tx_string(" Theta: ");
m_usb_tx_int(theta*57.6);
m_usb_tx_string(" CX (cm): ");
m_usb_tx_int(cX);
m_usb_tx_string(" CY (cm): ");
m_usb_tx_int(cY);
m_usb_tx_string(" Count: ");
m_usb_tx_int(count);
m_usb_tx_string(" Toangle_C: ");
m_usb_tx_int(to_angle_c*57.6);
m_usb_tx_string(" Toangle_1: ");
m_usb_tx_int(to_angle_1*57.6);
m_usb_tx_string(" Toangle_2: ");
m_usb_tx_int(to_angle_2*57.6);
m_usb_tx_string(" Toangle: ");
m_usb_tx_int(to_angle*57.6);
m_usb_tx_string("\n");
m_wait(10);
} ///while (1)
}// main void()
void gotogoalf(void)
{
sei();
localizef();
if (XRobot < 0) {
x_target = 280;
y_target = 0 ;
clearDisplay();
// m_port_set(0x20, PORTG,1); //Display 2
// m_port_set(0x20, PORTG,2);
// m_port_set(0x20, PORTG,3);
// m_port_set(0x20, PORTG,4);
// m_port_set(0x20, PORTG,6);
}
else {
x_target = -280;
y_target = 0;
clearDisplay();
// m_port_set(0x20, PORTG,1); //Display 3
// m_port_set(0x20, PORTG,3);
// m_port_set(0x20, PORTG,4);
// m_port_set(0x20, PORTG,4);
// m_port_set(0x20, PORTG,6);
}
deltay = y_target-YRobot;
deltax = x_target-XRobot;
theta_target = atan2(deltay, deltax)+pi;
if (theta_target > pi) {
theta_target = theta_target - 2*pi;
}
theta_target_d = theta_target*57.296;
//theta_rotate = theta_target - Theta;
theta_rotate_d = theta_target_d - Theta_d;
//m_usb_tx_int(Theta_d);
//m_usb_tx_string(" ");
//m_usb_tx_int(theta_target_d);
//m_usb_tx_string(" ");
//m_usb_tx_int(theta_rotate_d);
//m_usb_tx_string(" ");
//m_usb_tx_int(XRobot);
//m_usb_tx_string(" ");
//m_usb_tx_int(YRobot);
//m_usb_tx_string("\n");
// Determine motor directions
if (theta_rotate_d < 0) {
// Turn clockwise
clear(PORTB,5); // For Dom, need to CLEAR B5 to move forwards (for Vinnie it's SET)
clear(PORTB,3);
clearDisplay();
m_port_set(0x20, PORTG,0); //Display 5
m_port_set(0x20, PORTG,1);
m_port_set(0x20, PORTG,3);
m_port_set(0x20, PORTG,4);
m_port_set(0x20, PORTG,4);
} else {
// Turn counter-clockwise
set(PORTB,5); // For Dom, need to CLEAR B5 to move forwards (for Vinnie it's SET)
set(PORTB,3);
clearDisplay();
m_port_set(0x20, PORTG,0); //Display 6
m_port_set(0x20, PORTG,1);
m_port_set(0x20, PORTG,2);
m_port_set(0x20, PORTG,3);
m_port_set(0x20, PORTG,4);
}
// Turn motor until current angle matches target angle
while (Theta_d > theta_target_d + tol || Theta_d < theta_target_d - tol) {
sei();
OCR1B = 5990;
OCR1C = 5990;
localizef();
clearDisplay();
m_port_set(0x20, PORTG,4); //Display 7
m_port_set(0x20, PORTG,4);
m_port_set(0x20, PORTG,6);
}
clearDisplay();
// Move bot forward
OCR1B = 16000;
OCR1C = 16000;
clear(PORTB,5); // For Dom, need to CLEAR B5 to move forwards (for Vinnie it's SET)
set(PORTB,3);
// Turn motor until current coordinates match target coordinates
if(x_target < 0){
while (XRobot > x_target - 30) { // ADD TOLERANCE *******
sei();
m_wait(8);
localizef();
}
}else{
while (XRobot < x_target - 30) { // ADD TOLERANCE *******
sei();
m_wait(8);
localizef();
}
}
// Stop motors
OCR1B = 0;
OCR1C = 0;
clear(PORTB,6);
clear(PORTB,7);
clearDisplay();
m_port_set(0x20, PORTG,4); //Display 1
m_port_set(0x20, PORTG,6);
}
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);
}
}
}
}
int main(void)
{
m_clockdivide(0); // clock speed 16 MHz
sei(); // enable global interrups
int derivative = 0;
// 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
int side; // integer to figure out which side we're on
while (1)
{
// USB print testing
m_usb_tx_string("testing: ");
m_usb_tx_int(testing);
m_usb_tx_string(" ");
m_usb_tx_string("\n");
// continue code if ply command is issued
if ((unsigned char)buffer1[0] == 0xA1){
break;
}
}
// turn red light on if play command is issued
m_red(ON);
// wait 10 seconds to let M_Wii start recording stars
m_wait(1000);
int position[3]; // array for robot position
int x1; // x position
int i;
// calculate initial positions
for(i = 0; i<1000; i++)
{
current_location(position);
x1 = position[0];
}
// figure out what side robot starts on
if (x1 > 0)
{
side = -1;
}
else
{
side = 1;
}
// array to hold constants
float constants[4];
//goal();
// main LOOP
while(1)
{
// calculate positions
int b = current_location(position);
int x1 = position[0];
int y1 = position[1];
int angle = position[2];
// usb prinitng
m_usb_tx_string("LOCALIZATION: ");
m_usb_tx_int(b);
m_usb_tx_string(" ");
m_usb_tx_string("X Position: ");
m_usb_tx_int(x1);
m_usb_tx_string(" ");
m_usb_tx_string("Y Position: ");
m_usb_tx_int(y1);
m_usb_tx_string(" ");
m_usb_tx_string("Angle: ");
m_usb_tx_int(angle);
m_usb_tx_string(" ");
m_usb_tx_string("\n");
// get constants
if (side>0){
get_constants1(constants, -115, 0, x1, y1, angle, side, derivative);
}
else
{
get_constants1(constants, 115, 0, x1, y1, angle, side, derivative);
}
float p = constants[0]; // speed constant
int d = (int)constants[1]; // direction constant
float j = constants[2]; // how much turn constant
derivative = constants[3];
turn_robot( d, 1, .9, j); // DRIVE THE BOT
}
}
void updown(int what, int nr)
{
#ifdef LOG_XFER
#warning LOG_XFER defined!
FILE *xfl;
#endif
const char *name[13];
int idx[13];
int r, f, g = 0;
char *t = what == 'U' ? _("Upload") : _("Download");
char buf[160];
char buffirst[20];
char xfrstr[160] = "";
char trimbuf[160] = "";
char title[64];
const char *s ="";
int pipefd[2];
int n, status;
char * cmdline = NULL;
char * translated_cmdline = NULL;
WIN *win = (WIN *)NULL;
if (mcd(what == 'U' ? P_UPDIR : P_DOWNDIR) < 0)
return;
/* Automatic? */
if (nr == 0) {
for (f = 0; f < 12; f++) {
if (P_PNAME(f)[0] && P_PUD(f) == what) {
name[g] = P_PNAME(f);
idx[g++] = f;
}
}
name[g] = NULL;
if (g == 0)
return;
r = mc_wselect(30, 7, name, NULL, t, stdattr, mfcolor, mbcolor) - 1;
if (r < 0)
return;
g = idx[r];
} else
g = nr;
buf[0] = 0;
/* jseymour file selector with choice of dir on zmodem, etc. download */
#if 1
{
int multiple; /* 0:only directory, 1:one file, -1:any number */
size_t cmdline_length;
if (P_MUL(g)=='Y')
/* need file(s), or just a directory? */
multiple = what == 'U'? -1 : 0;
else
multiple = 1; /* only one allowed */
if (P_FSELW[0] == 'Y' && (what == 'U' || P_ASKDNDIR[0] == 'Y')) {
s = filedir(multiple, what == 'U'? 0 : 1);
if (s == NULL)
return;
}
else if (P_PNN(g) == 'Y') {
s = input(_("Please enter file names"), buf);
if (s == NULL)
return;
}
/* discard directory if "multiple" == 0 */
cmdline_length = strlen(P_PPROG(g)) + strlen((char*) (multiple == 0 ? "" : s)) + 1; /* + 1 for ' ' */
cmdline = malloc(cmdline_length + 1); /* + 1 for NUL */
if (cmdline == NULL) {
werror(_("Out of memory: could allocate buffer for command line"));
return;
}
snprintf(cmdline, cmdline_length + 1, "%s %s", P_PPROG(g), multiple == 0 ? "" : s);
}
#endif
if (P_LOGXFER[0] == 'Y')
do_log("%s", cmdline); /* jl 22.06.97 */
if (P_PFULL(g) == 'N') {
win = mc_wopen(10, 7, 70, 13, BSINGLE, stdattr, mfcolor, mbcolor, 1, 0, 1);
snprintf(title, sizeof(title), _("%.30s %s - Press CTRL-C to quit"), P_PNAME(g),
what == 'U' ? _("upload") : _("download"));
mc_wtitle(win, TMID, title);
pipe(pipefd);
} else
mc_wleave();
m_flush(portfd);
switch (udpid = fork()) {
case -1:
werror(_("Out of memory: could not fork()"));
if (win) {
close(pipefd[0]);
close(pipefd[1]);
mc_wclose(win, 1);
} else
mc_wreturn();
mcd("");
if(cmdline)
free(cmdline);
return;
case 0: /* Child */
if (P_PIORED(g) == 'Y') {
dup2(portfd, 0);
dup2(portfd, 1);
}
if (win) {
dup2(pipefd[1], 2);
close(pipefd[0]);
if (pipefd[1] != 2)
close(pipefd[1]);
}
lockfile_remove();
for (n = 1; n < _NSIG; n++)
signal(n, SIG_DFL);
translated_cmdline = translate(cmdline);
if (translated_cmdline != NULL) {
fastexec(translated_cmdline);
free(translated_cmdline);
}
if(cmdline)
free(cmdline);
exit(1);
default: /* Parent */
break;
}
if(cmdline)
free(cmdline);
if (win) {
setcbreak(1); /* Cbreak, no echo. */
enab_sig(1, 0); /* But enable SIGINT */
}
signal(SIGINT, udcatch);
if (P_PIORED(g) == 'Y') {
close(pipefd[1]);
#ifdef LOG_XFER
xfl=fopen("xfer.log","wb");
#endif
while ((n = read(pipefd[0], buf, sizeof(buf))) > 0) {
buf[n] = '\0';
mc_wputs(win, buf);
timer_update();
/* Log the filenames & sizes jl 14.09.97 */
if (P_LOGXFER[0] == 'Y') {
#ifdef LOG_XFER
if (xfl)
fprintf(xfl,">%s<\n",buf);
#endif
if (sscanf(buf, "%19s", buffirst)) { /* if / jl 29.09.97 */
if (!strncmp (buffirst, "Receiving", 9) ||
!strncmp (buffirst, "Sending", 7)) {
if (xfrstr[0]) {
trim (trimbuf, xfrstr, sizeof(trimbuf));
do_log ("%s", trimbuf);
xfrstr[0] = 0;
}
trim (trimbuf, buf, sizeof(trimbuf));
do_log("%s", trimbuf);
} else if (!strncmp (buffirst, "Bytes", 5)) {
strncpy (xfrstr, buf, sizeof(xfrstr));
}
buffirst[0] = 0;
trimbuf[0] = 0;
}
}
}
#ifdef LOG_XFER
if (xfl)
fclose(xfl);
#endif
}
/* Log the last file size jl 14.09.97 */
if (P_LOGXFER[0] == 'Y' && xfrstr[0]) {
trim (trimbuf, xfrstr, sizeof(trimbuf));
do_log ("%s", trimbuf);
xfrstr[0] = 0;
}
while (udpid != m_wait(&status));
if (win) {
enab_sig(0, 0);
signal(SIGINT, SIG_IGN);
}
if (win == (WIN *)0)
mc_wreturn();
lockfile_create();
/* MARK updated 02/17/94 - Flush modem port before displaying READY msg */
/* because a BBS often displays menu text right after a download, and we */
/* don't want the modem buffer to be lost while waiting for key to be hit */
m_flush(portfd);
port_init();
setcbreak(2); /* Raw, no echo. */
if (win)
close(pipefd[0]);
mcd("");
timer_update();
/* If we got interrupted, status != 0 */
if (win && (status & 0xFF00) == 0) {
#if VC_MUSIC
if (P_SOUND[0] == 'Y') {
mc_wprintf(win, _("\n READY: press any key to continue..."));
music();
} else
sleep(1);
#else
/* MARK updated 02/17/94 - If there was no VC_MUSIC capability, */
/* then at least make some beeps! */
if (P_SOUND[0] == 'Y')
mc_wprintf(win, "\007\007\007");
sleep(1);
#endif
}
if (win)
mc_wclose(win, 1);
}
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)
//{
//}
}
}
/*
* Run an external script.
* ask = 1 if first ask for confirmation.
* s = scriptname, l=loginname, p=password.
*/
void runscript(int ask, const char *s, const char *l, const char *p)
{
int status;
int n, i;
int pipefd[2];
char buf[81];
char scr_lines[5];
char cmdline[128];
struct pollfd fds[2];
char *translated_cmdline;
char *ptr;
WIN *w;
int done = 0;
char *msg = _("Same as last");
char *username = _(" A - Username :"******" B - Password :"******" C - Name of script :"),
*question = _("Change which setting? (Return to run, ESC to stop)");
if (ask) {
w = mc_wopen(10, 5, 70, 10, BDOUBLE, stdattr, mfcolor, mbcolor, 0, 0, 1);
mc_wtitle(w, TMID, _("Run a script"));
mc_wputs(w, "\n");
mc_wprintf(w, "%s %s\n", username, scr_user[0] ? msg : "");
mc_wprintf(w, "%s %s\n", password, scr_passwd[0] ? msg : "");
mc_wprintf(w, "%s %s\n", name_of_script, scr_name);
mc_wlocate(w, 4, 5);
mc_wputs(w, question);
mc_wredraw(w, 1);
while (!done) {
mc_wlocate(w, mbslen (question) + 5, 5);
n = wxgetch();
if (islower(n))
n = toupper(n);
switch (n) {
case '\r':
case '\n':
if (scr_name[0] == '\0') {
mc_wbell();
break;
}
mc_wclose(w, 1);
done = 1;
break;
case 27: /* ESC */
mc_wclose(w, 1);
return;
case 'A':
mc_wlocate(w, mbslen (username) + 1, 1);
mc_wclreol(w);
scr_user[0] = 0;
mc_wgets(w, scr_user, 32, 32);
break;
case 'B':
mc_wlocate(w, mbslen (password) + 1, 2);
mc_wclreol(w);
scr_passwd[0] = 0;
mc_wgets(w, scr_passwd, 32, 32);
break;
case 'C':
mc_wlocate(w, mbslen (name_of_script) + 1, 3);
mc_wgets(w, scr_name, 32, 32);
break;
default:
break;
}
}
} else {
strncpy(scr_user, l, sizeof(scr_user));
strncpy(scr_name, s, sizeof(scr_name));
strncpy(scr_passwd, p, sizeof(scr_passwd));
}
sprintf(scr_lines, "%d", (int) lines); /* jl 13.09.97 */
/* Throw away status line if temporary */
if (tempst) {
mc_wclose(st, 1);
tempst = 0;
st = NULL;
}
scriptname(scr_name);
pipe(pipefd);
if (mcd(P_SCRIPTDIR) < 0)
return;
snprintf(cmdline, sizeof(cmdline), "%s %s %s %s",
P_SCRIPTPROG, scr_name, logfname, logfname[0]==0? "": homedir);
switch (udpid = fork()) {
case -1:
werror(_("Out of memory: could not fork()"));
close(pipefd[0]);
close(pipefd[1]);
mcd("");
return;
case 0: /* Child */
dup2(portfd, 0);
dup2(portfd, 1);
dup2(pipefd[1], 2);
close(pipefd[0]);
close(pipefd[1]);
for (n = 1; n < _NSIG; n++)
signal(n, SIG_DFL);
mc_setenv("LOGIN", scr_user);
mc_setenv("PASS", scr_passwd);
mc_setenv("TERMLIN", scr_lines); /* jl 13.09.97 */
translated_cmdline = translate(cmdline);
if (translated_cmdline != NULL) {
fastexec(translated_cmdline);
free(translated_cmdline);
}
exit(1);
default: /* Parent */
break;
}
setcbreak(1); /* Cbreak, no echo */
enab_sig(1, 0); /* But enable SIGINT */
signal(SIGINT, udcatch);
close(pipefd[1]);
/* pipe output from "runscript" program to terminal emulator */
fds[0].fd = pipefd[0]; /* runscript */
fds[0].events = POLLIN;
fds[1].fd = STDIN_FILENO; /* stdin */
fds[1].events = POLLIN;
script_running = 1;
while (script_running && poll(fds, 2, -1) > 0)
for (i = 0; i < 2; i++) {
if (fds[i].revents & (POLLERR | POLLHUP | POLLNVAL))
script_running = 0;
else if ((fds[i].revents & POLLIN)
&& (n = read(fds[i].fd, buf, sizeof(buf)-1)) > 0) {
ptr = buf;
while (n--)
if (i)
vt_send(*ptr++);
else
vt_out(*ptr++);
timer_update();
mc_wflush();
}
}
/* Collect status, and clean up. */
m_wait(&status);
enab_sig(0, 0);
signal(SIGINT, SIG_IGN);
setcbreak(2); /* Raw, no echo */
close(pipefd[0]);
scriptname("");
mcd("");
}
void playHelper(){
// GET DIR
if(check(PINB,5)){
direction=true; //SET THE ROBOT DIRECTION TO SHOOT
} else {
direction=false;
}
// direction = directionPacket;
// READ ADC VALS
int adc1 = ADC0_read();
int adc2 = ADC1_read();
//m_usb_tx_string("adc1, adc2 = (");
//m_usb_tx_int(adc1);
//m_usb_tx_string(", ");
//m_usb_tx_int(adc2);
//m_usb_tx_string(")\n");
int adcDiff = adc2 - adc1;
if(direction) m_usb_tx_string("direction true\n");
else m_usb_tx_string("direction false\n");
if(timer>=1){
turnLeft();
m_wait(1000);
backward();
m_wait(500);
timer = 0;
} else if(hasPuck()){// ROBOT SWITCH PUSHED.. PUCK IS SEEN
m_green(OFF);
m_wait(100);
unsigned int xTarg;
unsigned int yTarg = 60;
if(direction){ // Face goal A
xTarg = 240;
} else { // Face goal B
xTarg = 0;
}
// ROTATE TO FACE GOAL
if(!facing(xTarg,yTarg)) return;
if(direction){ // Try to go to A
if(myPos[X]>160){ // Close enough, fire!
m_green(ON);
fire();
} else {
m_green(OFF); // Too far, keep going forward
forward();
}
} else { // Try to go to B
if(myPos[X]<80){ // Close enough, fire!
m_green(ON);
fire();
} else { // Too far, keep going forward
m_green(OFF);
forward();
}
}
} else if(adc1>960||adc2>960){ // PUCK SEEN, hone in..
if(adcDiff>10){
spinLeft();
} else if(adcDiff<-10){
spinRight();
} else {
forward();
}
} else{ // PUCK NOT SEEN, keep spinning..
m_green(OFF);
spinRight();
}
}
void playStriker(){
// GET DIR
if(check(PINB,5)){
direction=true; //SET THE ROBOT DIRECTION TO SHOOT
} else {
direction=false;
}
// GET POS
getPos();
// SEND LOCATION TO HELPER
//buffer[1]=myPos[X];
//buffer[2]=myPos[Y];
//buffer[3]=direction;
//m_rf_send(TXADDRESS, buffer, PACKET_LENGTH);
// READ ADC VALS
int adc1 = ADC0_read();
int adc2 = ADC1_read();
// m_usb_tx_string("adc1, adc2 = (");
// m_usb_tx_int(adc1);
// m_usb_tx_string(", ");
// m_usb_tx_int(adc2);
// m_usb_tx_string(")\n");
int adcDiff = adc2 - adc1;
// TIMER CHECKS
//m_usb_tx_string("myLastPos(x,y): (");
//m_usb_tx_int(myLastPos[X]);
//m_usb_tx_string(", ");
//m_usb_tx_int(myLastPos[Y]);
//m_usb_tx_string(") TIMER:");
//m_usb_tx_int(timer);
//m_usb_tx_string("\n");
if(timer>5 && timer<10){
backward();
m_wait(250);
turnLeft();
m_wait(250);
backward();
m_wait(250);
timer = 0;
} else if(hasPuck()){// ROBOT SWITCH PUSHED.. PUCK IS SEEN
m_green(OFF);
m_wait(100);
unsigned int xTarg;
unsigned int yTarg = 60;
if(direction){ // Face goal A
xTarg = 240;
} else { // Face goal B
xTarg = 0;
}
// ROTATE TO FACE GOAL
if(!facing(xTarg,yTarg)) return;
if(direction){ // Try to go to A
if(myPos[X]>160){ // Close enough, fire!
m_green(ON);
fire();
} else {
m_green(OFF); // Too far, keep going forward
forward();
}
} else { // Try to go to B
if(myPos[X]<80){ // Close enough, fire!
m_green(ON);
fire();
} else { // Too far, keep going forward
m_green(OFF);
forward();
}
}
} else if(adc1>960||adc2>960){ // PUCK SEEN, hone in..
if(adcDiff>10){
spinLeft();
} else if(adcDiff<-10){
spinRight();
} else {
forward();
}
} else{ //PUCK NOT SEEN, keep spinning..
m_green(OFF);
spinRight();
}
}
int main(void)
{
/* insert your hardware initialization here */
InitPeripherals();
#ifdef MASTER
USART USART_CHANNEL;
USART_CHANNEL.USART_BAUDRATE = 9600;
USART_CHANNEL.USART_RECEIVER_STATUS = RECEIVER_ENABLE;
USART_CHANNEL.USART_TRANSMITTER_STATUS = TRANSMITTER_ENABLE;
USART_CHANNEL.USART_STOP_BIT = 1;
USART_CHANNEL.USART_PARITY = PARITY_EVEN;
USART_Init(USART_CHANNEL);
clear(TCCR1B, CS12);
clear(TCCR1B, CS11);
set(TCCR1B, CS10); // prescalar is 1
set(TCCR1B, WGM13);
set(TCCR1B, WGM12);
set(TCCR1A, WGM11);
clear(TCCR1A, WGM10);
set(TCCR1A, COM1A1);
set(TCCR1A, COM1A0);
set(DDRB, 1);
ICR1 = (int)(8000000/500000) - 1;
OCR1A = (int)((float)ICR1 * 0.5); // common minus output 500KHz signals
set(DDRD, 5);
set(PORTD, 5); // COIL3 PLUS on
// unsigned long frequency = 500000;
// float duty_cycle = 0.5;
// unsigned int pulse_number = 3;
// init_coil(frequency, duty_cycle, pulse_number);
//
// COIL3_PLUS_ON;
//
// drive_coil_communication();
sei();
#endif
#ifdef SLAVE
// unsigned long frequency = 500000;
// float duty_cycle = 0.5;
// unsigned int pulse_number = 3;
// init_coil(frequency, duty_cycle, pulse_number);
// COIL1_MINUS_ON;
set(DDRD, 4);
set(PORTD, 4); // COIL2_MINUS on
set(DDRD, 1);
set(PORTD, 1); // USART TX is high
set(DDRB, 3);
set(PORTB, 3); // turn on receiver
#endif
m_green(ON);m_yellow(ON);m_blue(ON);
m_wait(1000);
m_green(OFF);m_yellow(OFF);m_blue(OFF);
m_wait(2000);
for(;;){
/* insert your main loop code here */
#ifdef MASTER
set(PORTD, 5);
USART_Transmit_string("MODLAB!\n\r");
clear(PORTD, 5);
m_wait(1000);
#endif
#ifdef SLAVE
m_blue(TOGGLE);
#endif
}
return 0; /* never reached */
}
