void disp4(void)
{
m_port_set(0x20, PORTG,0); //Display 4
m_port_set(0x20, PORTG,1);
m_port_set(0x20, PORTG,4);
m_port_set(0x20, PORTG,6);
}
void disp2(void)
{ 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,5);
}
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;
}
}
}
void dispN(void)
{
//Display n
m_port_set(0x20, PORTG,1);
m_port_set(0x20, PORTG,2);
m_port_set(0x20, PORTG,6);
}
void dispC(void)
{
m_port_set(0x20, PORTG,0); //Display C
m_port_set(0x20, PORTG,2);
m_port_set(0x20, PORTG,3);
m_port_set(0x20, PORTG,4);
}
void dispO(void)
{
m_port_set(0x20, PORTG,1); //Display o
m_port_set(0x20, PORTG,2);
m_port_set(0x20, PORTG,3);
m_port_set(0x20, PORTG,6);
}
void dispE(void)
{
m_port_set(0x20, PORTG,0); //Display E
m_port_set(0x20, PORTG,1);
m_port_set(0x20, PORTG,2);
m_port_set(0x20, PORTG,3);
m_port_set(0x20, PORTG,4);
}
void disp9(void)
{
m_port_set(0x20, PORTG,0); //Display 9
m_port_set(0x20, PORTG,1);
m_port_set(0x20, PORTG,4);
m_port_set(0x20, PORTG,5);
m_port_set(0x20, PORTG,6);
}
void dispF(void)
{
m_port_set(0x20, PORTG,0); //Display F
m_port_set(0x20, PORTG,1);
m_port_set(0x20, PORTG,2);
m_port_set(0x20, PORTG,4);
}
void disp3(void)
{
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,5);
m_port_set(0x20, PORTG,6);
}
void disp6(void)
{
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,6);
}
void playf(void)
{ clearDisplay();
m_port_set(0x20, PORTG,4); //Display 1
m_port_set(0x20, PORTG,6);
state = 0x00;
}
void dispY(void)
{
m_port_set(0x20, PORTG,0); //Display Y
m_port_set(0x20, PORTG,1);
m_port_set(0x20, PORTG,3);
m_port_set(0x20, PORTG,4);
m_port_set(0x20, PORTG,6);
}
void dispP(void)
{
m_port_set(0x20, PORTG,0); //Display P
m_port_set(0x20, PORTG,1);
m_port_set(0x20, PORTG,2);
m_port_set(0x20, PORTG,4);
m_port_set(0x20, PORTG,5);
}
void dispU(void)
{
m_port_set(0x20, PORTG,0); //Display U
m_port_set(0x20, PORTG,2);
m_port_set(0x20, PORTG,3);
m_port_set(0x20, PORTG,4);
m_port_set(0x20, PORTG,6);
}
void dispH(void)
{
m_port_set(0x20, PORTG,0); //Display 8
m_port_set(0x20, PORTG,1);
m_port_set(0x20, PORTG,2);
m_port_set(0x20, PORTG,4);
m_port_set(0x20, PORTG,6);
}
void dispD(void)
{
//Display d
m_port_set(0x20, PORTG,1);
m_port_set(0x20, PORTG,2);
m_port_set(0x20, PORTG,3);
m_port_set(0x20, PORTG,4);
m_port_set(0x20, PORTG,6);
}
void pausef(void)
{ clearDisplay();
m_port_set(0x20, PORTG,0); //Display 4
m_port_set(0x20, PORTG,1);
m_port_set(0x20, PORTG,4);
m_port_set(0x20, PORTG,6);
state = 0x00;
OCR1B = 0;
OCR1C = 0;
while(1){
sei();
}
}
void disp0(void)
{
m_port_set(0x20, PORTG,0); //Display 0
m_port_set(0x20, PORTG,2);
m_port_set(0x20, PORTG,3);
m_port_set(0x20, PORTG,4);
m_port_set(0x20, PORTG,5);
m_port_set(0x20, PORTG,6);
}
void dispA(void)
{
m_port_set(0x20, PORTG,0); //Display A
m_port_set(0x20, PORTG,1);
m_port_set(0x20, PORTG,2);
m_port_set(0x20, PORTG,4);
m_port_set(0x20, PORTG,5);
m_port_set(0x20, PORTG,6);
}
void dispG(void)
{
m_port_set(0x20, PORTG,0); //Display g
m_port_set(0x20, PORTG,1);
m_port_set(0x20, PORTG,3);
m_port_set(0x20, PORTG,4);
m_port_set(0x20, PORTG,5);
m_port_set(0x20, PORTG,6);
}
void disp1(void)
{
m_port_set(0x20, PORTG,4); //Display 1
m_port_set(0x20, PORTG,6);
}
int main(void)
{ m_clockdivide(0);
m_bus_init();
m_usb_init();
m_rf_open(CHANNEL,RXADDRESS,LENGTH);
m_wii_open();
sei();
OCR1A = 31999;
OCR1B = 0;
OCR1C = 0;
m_disableJTAG();
clear(ADMUX,REFS1); // Setting the reference voltage for the ADC to Vcc
set(ADMUX,REFS0); // ^
set(ADCSRA,ADPS2); // set ADC prescaler to 128
set(ADCSRA,ADPS1); // ^
set(ADCSRA,ADPS0); //^
set(DIDR0,ADC0D); // disable digital on F0 (ADC0)
set(DIDR0,ADC1D); // disable digital to F1
set(DIDR0,ADC4D); //disable digital to F4
set(DIDR0,ADC5D); //disable digital to F5
set(DIDR0,ADC6D); //disable digital to F6
set(DIDR0,ADC7D); //disable digital to F7
set(ADMUX,REFS0); // set ADC reference to Vcc
set(ADCSRA,ADIE); //Enables interrups when conversion finishes
// clear(ADMUX,MUX2); // This is the front right. Range 50-1023. Port is F1
// set(ADMUX,MUX0);
set(TCCR1B,WGM13); // Mode 15: up to OCR1A, PWM, single slope
set(TCCR1B,WGM12); // ^
set(TCCR1A,WGM11); // ^
set(TCCR1A,WGM10); // ^
set(TCCR1A,COM1B1); //clear at OCR1B, set at rollover
clear(TCCR1A,COM1B0); // ^
set(TCCR1A,COM1C1); // clear at OCR1C, set at rollover
clear(TCCR1A,COM1C0);
clear(TCCR1B,CS12); // set clock pre-scaler to /1
clear(TCCR1B,CS11); // ^
set(TCCR1B,CS10); // ^
m_port_init(0x20); // Initializes the port expander and sets all of the G pins as outputs
m_port_set(0x20, DDRG,0);
m_port_set(0x20, DDRG,1);
m_port_set(0x20, DDRG,2);
m_port_set(0x20, DDRG,3);
m_port_set(0x20, DDRG,4);
m_port_set(0x20, DDRG,5);
m_port_set(0x20, DDRG,6);
m_port_set(0x20, DDRG,7);
clearDisplay();
// Set motor output ports
set(DDRB,6);
set(DDRB,7);
set(DDRB,5);
set(DDRB,3);
// Turn off motors
clear(PORTB,6);
clear(PORTB,7);
set(ADCSRA,ADEN); // enable ADC subsystem
set(ADCSRA,ADSC); // start ADC conversion
while(1){
switch(state){
case COMM:
commtestf();
break;
case PAUSE:
pausef();
break;
case PLAY:
playf();
localizef();
defendf();
//puckfindf();
break;
case GOTOGOAL:
gotogoalf();
case HALFTIME:
pausef();
break;
case GOALA:
pausef();
break;
case GOALB:
pausef();
break;
case GAMEOVER:
pausef();
break;
default:
break;
}
defendf();
m_usb_tx_int(photo1);
m_usb_tx_string(" ");
m_usb_tx_int(photo2);
m_usb_tx_string(" ");
m_usb_tx_int(photo3);
m_usb_tx_string(" ");
m_usb_tx_int(photo4);
m_usb_tx_string(" ");
m_usb_tx_int(photo5);
m_usb_tx_string("\n");
}
}
void disp7(void)
{
m_port_set(0x20, PORTG,4); //Display 7
m_port_set(0x20, PORTG,5);
m_port_set(0x20, PORTG,6);
}
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 dispI(void)
{ m_port_set(0x20, PORTG,0); //Display 8
m_port_set(0x20, PORTG,2);
}
void dispJ(void)
{
m_port_set(0x20, PORTG,3);
m_port_set(0x20, PORTG,4);
m_port_set(0x20, PORTG,6);
}
void dispL(void)
{
m_port_set(0x20, PORTG,0); //Display L
m_port_set(0x20, PORTG,2);
m_port_set(0x20, PORTG,3);
}
