int main(void) {
//initialize mBUS
m_bus_init();
m_usb_init();
/* m_port_init(port_num);
m_port_set(port_num,DDRG,PIN_Comm);
m_port_set(port_num,DDRG,PIN_Play);
m_port_set(port_num,DDRG,PIN_Stop);
*/
set(DDRB,1);
set(DDRB,2);
set(DDRB,3);
m_clockdivide(0);
m_rf_open(chan,address,p_len);
sei();
while (1) {
if (comm_cmd == true) {
get_command();
}
}
}
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
}
}
}
// --------------------------------------------------------------
// Initialize M2
// --------------------------------------------------------------
void init(void){
// Initialize RED LED as on for the Initialization of M2
m_red(ON);
// Set the System Clock at 16MHz
m_clockdivide(0);
// Initialize the mBus
m_bus_init();
if (USB) {
m_usb_init(); // Initialize USB and
while(!m_usb_isconnected()); // wait for a connection
}
m_rf_open(CHANNEL,RXADDRESS,PACKET_LENGTH);
m_red(OFF);
// Turn off the RED LED when done
}
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 init(){
m_usb_init(); //initialize usb communication
while(!m_usb_isconnected()); // wait for a connection
m_clockdivide(0); // 16Mhz
m_red (OFF); // Start with the LED off
// Voltage Reference
clear(ADMUX,REFS1); // Vcc
set(ADMUX,REFS0); // ^
// ADC Prescalar
set(ADCSRA,ADPS2); // prescaler 128 to 125kHz
set(ADCSRA,ADPS1); // ^
set(ADCSRA,ADPS0); // ^
// Disable other inputs
set(DIDR0,ADC0D); // disable F0
set(DIDR0,ADC1D); // disable F1
set(DIDR0,ADC4D); // disable F4
set(DIDR0,ADC5D); // disable F5
set(DIDR0,ADC6D); // disable F6
set(DIDR0,ADC7D); // disable F7
// Set Desired Pin Input
clear(ADCSRB, MUX5); //set pin to F0
clear(ADCSRB, MUX2); //^
clear(ADCSRB, MUX1); //^
clear(ADCSRB, MUX0); //^
// Enable Conversion
set(ADCSRA,ADEN); // enable conversion
}
void main(void)
{
sei();
OCR0A = 50;
m_usb_init();
m_clockdivide(0);
init();
init2();
m_bus_init();
m_rf_open(1,0x4E,3);
while(1)
{
m_usb_tx_string(" j value ");
m_usb_tx_int(j);
m_usb_tx_string(" Buffer value ");
m_usb_tx_int(buffer[2]);
m_usb_tx_string("\n");
if(j == 30000)
{
j = 15000;
}
}
}
int main(void)
{
unsigned int value;
m_usb_init();
while(!m_usb_isconnected()); // wait for a connection
while(1) {
if(m_usb_rx_available()) {
value = m_usb_rx_char();
m_usb_tx_uint(value);
}
}
}
// Public functions
// Initialize peripheral devices
void robockey_init(void)
{
m_clockdivide(0); // 16 MHz
sei(); // Enable global interrupt
timer1_init(0.5); // kHz
timer3_init(50); // Hz
//timer0_init(1); // kHz
adc_init();
mode_init();
m_rf_open(CHANNEL,RXADDRESS,PACKET_LENGTH_RF); // Enable wireless
m_usb_init(); // Enable usb communication
m_bus_init(); // Enable mbus
m_wii_open(); // Enable wii camera
}
void sniper()
{
init_all();
// 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,ADC1D);//setting F1
set(DIDR0,ADC4D);//setting F4
set(DIDR0,ADC5D);//setting F5
set(DIDR0,ADC6D);//setting F6
set(DIDR0,ADC7D);//setting F7
set(ADCSRA, ADEN);
play_state = 1;
set_position(1024/2, 768/2);
first = true;
//state_before_game();
//state_play();
m_usb_init();
/* while(!m_usb_isconnected());
m_green(ON);
*/
set_left(30);
set_right(100);
while(1)
{
adc();
// m_usb_tx_string("left: ");
//m_usb_tx_int(a_left);
wait(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 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(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');
}
}
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);
}
}
}
}
/*char isStuck() {
m_wait(100);
//localize(data);
distfrombound = data[1] + ((float)BOUNDARYTHRESHOLD)*sin((data[2]+90.0)*3.14/180.0*-1.0);
if (distfrombound > 60.0 || distfrombound < -60.0) {
return 1;
}
m_usb_tx_int((int)distfrombound);
m_usb_tx_char('\n');
}
long loccounter = 0;
float prevx = 0.0;
float prevy = 0.0;
*/
int main(void)
{
//goal side
clear(DDRB,1);
clear(PORTB,1);
if (check(PINB,1)) {
goal = 1;
}
set(DDRB,0);
set(PORTB,0);
set(DDRD,5);
set(DDRD,3);
//wireless stuffs
m_bus_init();
m_rf_open(CHANNEL, RXADDRESS, PACKET_LENGTH);
int counter = 0;
//
//m_num_init();
int flag;
m_clockdivide(0);
m_disableJTAG();
//TIMER 0: For Controlling the solenoid
//
// set(TCCR0B, WGM02);
// set(TCCR0A, WGM01);
// set(TCCR0A, WGM01);
//
// set(TCCR0A, COM0B1);
// clear(TCCR0A, COM0B0);
//
// set(TCCR0B, CS02);
// set(TCCR0B, CS01);
// set(TCCR0B, CS00);
//
set(DDRB,7);
// OCR0A = 0xFF;
// OCR0B = 0xff;
//
//TIMER 1: For Controlling the left wheel
set(TCCR1B, WGM13);
set(TCCR1B, WGM12);
set(TCCR1A, WGM11);
set(TCCR1A, WGM10);
set(TCCR1A, COM1B1);
clear(TCCR1A, COM1B0);
clear(TCCR1B, CS12);
clear(TCCR1B, CS11);
set(TCCR1B, CS10);
set(DDRB,6);
OCR1A = 0xFFFF;
OCR1B = 0;
//TIMER 3: For Controlling the right wheel
//up to ICR3, clear at OCR3A & set at rollover
set(TCCR3B, WGM33);
set(TCCR3B, WGM32);
set(TCCR3A, WGM31);
clear(TCCR3A, WGM30);
set(TCCR3A, COM3A1);
clear(TCCR3A, COM3A0);
clear(TCCR3B, CS32);
clear(TCCR3B, CS31);
set(TCCR3B, CS30);
ICR3 = 0xFFFF;
OCR3A = 0;
// //Set TCNT0 to go up to OCR0A
// clear(TCCR0B, WGM02);
// set (TCCR0A, WGM01);
// clear(TCCR0A, WGM00);
//
// // Don't change pin upon hitting OCR0B
// clear(TCCR0A, COM0A1);
// clear(TCCR0A, COM0A0);
//
// // Set clock scale to /1024
// set(TCCR0B, CS02);
// clear(TCCR0B, CS01);
// set(TCCR0B, CS00);
//
// // Set Frequency of readings to 1/SAMPLERATE; dt = 1/SAMPLERATE
// OCR0A = (unsigned int) ((float) F_CPU / 1024 / REPORTRATE);
// OCR0B = 1;
// Set up interrupt for reading values at sampling rate
//Pin for controlling solenoid pulse
set(DDRB,7);
//Pins for controlling speed of left and right wheel
set(DDRB,6);
set(DDRC,6);
//Pins for determining direction of wheels
set(DDRB,2);
set(DDRB,3);
//Blue LED for Comm Test
//set(DDRB,5);
//ADC's
sei(); //Set up interrupts
set(ADCSRA, ADIE);
clear(ADMUX, REFS1); //Voltage reference is AR pin (5V)
clear(ADMUX, REFS0); //^
set(ADCSRA, ADPS2); //Set scale to /128
set(ADCSRA, ADPS1); //^
set(ADCSRA, ADPS0); //^
set(DIDR0, ADC0D); //Disable digital input for F0
set(DIDR0, ADC1D),
set(DIDR0, ADC4D);
set(DIDR0, ADC5D);
set(DIDR0, ADC6D);
set(DIDR0, ADC7D);
set(DIDR2, ADC8D);
set(DIDR2, ADC9D);
set(ADCSRA, ADATE); //Set trigger to free-running mode
chooseInput(0);
set(ADCSRA, ADEN); //Enable/Start conversion
set(ADCSRA, ADSC); //^
set(ADCSRA, ADIF); //Enable reading results
//Limit Switch stuffs
// clear(DDRB,0); //set to input, RIGHT LIMIT SWITCH
// clear(DDRB,1); //set to input, LEFT LIMIT SWITCH
//
// clear(PORTB,0); //disable internal pull up resistor
// clear(PORTB,1); //disable internal pull up resistor
//int state; // state variable
state = 0; //set state
play = 0;
long count = 0;
if (state == 70) {
m_usb_init(); // connect usb
while(!m_usb_isconnected()); //wait for connection
}
//m_bus_init();
m_wii_open();
//m_usb_init();
m_red(ON);
local_init();
localize(data);
m_red(OFF);
//set(TIMSK0, OCIE0A);
while(1)
{
if (play) {m_red(ON);}
else {m_red(OFF);}
// m_wait(100);
// m_red(TOGGLE);
// localize(data);
/*if (loccounter == LOCCOUNT) {
if (sqrt((data[0]-prevx)*(data[0]-prevx)+(data[1]-prevy)-(data[1]-prevy)) < 1.0) {
m_red(ON);
reverse();
m_wait(100);
state = 6;
}
else {
m_red(OFF);
state = 2;
}
prevx = data[0];
prevy = data[1];
loccounter = 0;
}*/
//loccounter++;
// localize(data);
// locdata[1] = (char)data[0];
// locdata[2] = (char)data[1];
// toggle(PORTD,3);
changedState = 0;
angle_dif = 0;
//Detect weather we have the puck
getADC();
//if (!play) game_pause();
if (ADCarr[7] > 500) {
//set(PORTD,5);
//set(PORTD,5);
iHaveThePuck = 1;
m_green(ON);
if (play && goal == A) state = 3;
if (play && goal == B) state = 4;
} else {
//clear(PORTD,5);
iHaveThePuck = 0;
m_green(OFF);
if (play) state = 2;
}
if(iHaveThePuck && state == 2) {
//set(PORTB,5);
//drive_to_goalA();
}
else {
//clear(PORTB,5);
//if (state != 0) state = 2;
}
// if(isStuck()) {
// //set(PORTD,5);u
// }
// else {
// //clear(PORTD,5);
// }
//localize(data);
// if (check(PINB,0)) {
// set(PORTD,5);
// state = 0x1A;
// } else {
// clear(PORTD,5);
// state = 2;
// }
if (!play) state = buffer[0];
//switch states
switch (state) {
case -2:
getADC();
if (ADCarr[0] > 500) {
m_green(ON);
}
else {m_green(OFF)}
break;
case -1: //test Limit switches
//m_green(ON);
if (check(PINB,1)) {
m_green(ON);
}
else if (check(PINB,0)) {
m_red(ON);
}
else {
m_red(OFF);
m_green(OFF);
}
break;
case 0:
//drive_to_point2(-100,0);
game_pause();
break;
case 1:
findPuck();
break;
case 2:
//m_red(ON);
if (!iHaveThePuck) {
if (play)
drive_to_puck();
}
break;
case 3:
if (play)
drive_to_goalA();
break;
case 4:
if (play)
drive_to_goalB();
break;
case 5:
getADC();
if (ADCarr[7] > 500) {
//set(PORTD,5);
} else {
//clear(PORTD,5);
}
break;
case 6:
if (data[2] > 0) {
rotate(RIGHT,1);
} else {
rotate(LEFT,1);
}
break;
case 7:
drive_to_point2(-50,0);
break;
case 0xA0:
comm_test();
break;
case 0xA1:
play = 1;
drive_to_puck();
break;
case 0xA2:
play = 0;
game_pause();
break;
case 0xA3:
play = 0;
game_pause();
break;
case 0xA4:
play = 0;
game_pause();
break;
case 0xA6:
play = 0;
game_pause();
break;
case 0xA7:
game_pause();
break;
case 69:
set(PORTB,2);
set(PORTB,3);
OCR1B = OCR1A;
OCR3A = ICR3;
break;
case 70:
reportADC();
break;
default:
game_pause();
break;
}
}
}
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");
}
}
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_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 setup(void)
{
m_red(ON);
m_green(ON);
// ***********************************
// Clock Speed
// ***********************************
m_clockdivide(0); // Set to 16MHz ('0' = divide by 1)
// ***********************************
// Watchdog
// ***********************************
wdt_reset();
wdt_enable(WDTO_2S); // Watchdog timer has a 2s interrupt
// ***********************************
// Debugging
// ***********************************
if(DEBUG){
/* initialize m_usb com */
m_usb_init();
while(!m_usb_isconnected());
}
// ***********************************
// Timer 1 Setup
// ***********************************
// Timer 1 used to control PWM into motor 1 - left
clear(TCCR1B, CS12); // Turn Prescaler to /8
set(TCCR1B, CS11); // ^
clear(TCCR1B, CS10); // ^
set(TCCR1B, WGM13); // mode 15 - up to 0CR1A
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); // ^
set(DDRB, 6); // enable B6 as an output pin for PWM
set(DDRB, 7); // enable B7 as an output pin for PWM
OCR1A = FREQ; // Frequency of PWM
// ***********************************
// Timer 3 Setup
// ***********************************
// clear(TCCR3B, CS32); // Scale by 1
// clear(TCCR3B, CS31);
// set(TCCR3B, CS30);
clear(TCCR3B, CS32); // Scale by 8
set(TCCR3B, CS31);
clear(TCCR3B, CS30);
clear(TCCR3B, WGM33); // Timer Mode 0
clear(TCCR3B, WGM32);
clear(TCCR3A, WGM31);
clear(TCCR3A, WGM30);
// ***********************************
// External Interrupts
// ***********************************
// PCINT0 = D0 = CH3, throttle
// PCINT1 = D1 = CH4, direction
// PCINT2 = D2 = CH5, kill switch
clear(EIMSK,INT0); // PCINT0
clear(EIMSK,INT1); // PCINT1
clear(EIMSK,INT2); // PCINT2
// PCINT0: Interrupt on either falling or rising edge
clear(EICRA,ISC01);
set(EICRA,ISC00);
// PCINT1: Interrupt on either falling or rising edge
clear(EICRA,ISC11);
set(EICRA,ISC10);
// PCINT2: Interrupt on either falling or rising edge
clear(EICRA,ISC21);
set(EICRA,ISC20);
// Remove masks for the corresponding interrupt
set(EIMSK,INT0); // PCINT0
set(EIMSK,INT1); // PCINT1
set(EIMSK,INT2); // PCINT2
sei(); // Enable global interrupts
m_red(OFF);
m_green(OFF);
}
//c7 red
/// e6 blue
int main(void)
{
m_disableJTAG(); // Turn off JTAG
//m_red(ON); // check code runs
m_clockdivide(0); // clock speed 16 MHz
m_bus_init(); // initialize m_bus communications
m_usb_init(); // USB COM
setup(); // motor setup
ADC_setup(); // setup anlog conversions
// start wireless communications
m_rf_open(CHANNEL, RXADDRESS, PACKET_LENGTH);
m_wii_open(); // and open wii camera
sei(); // enable global interrups
set(DDRC,7);
set(DDRB,7);
int position[3]; // array for robot position
int x_curr; // current x position
int y_curr; // current y position
int theta_curr; // current angle
int i;
int a;
// x, y and theta values
int x_pos[11];
int y_pos[11];
int theta[11];
char send[10];
int side;
int x1;
// calculate initial positions
int status;
// int x;
// int y;
// int theta1;
int hasPuck = 0;
// main LOOP
while(1)
{
if (flag)
{
m_rf_read(buffer1, PACKET_LENGTH);
testing = (unsigned char)buffer1[0];
a = state(testing);
// m_rf_read(buffer, PACKET_LENGTH);
// x = (unsigned char)buffer[0];
// y = (unsigned char)buffer[1];
// theta1 = (unsigned char)buffer[2];
if (flag1){
for(i = 0; i<10; i++)
{
current_location(position);
x1 = position[0];
}
// figure out what side robot starts on
if (x1 > 0)
{
side = -1;
}
else
{
side = 1;
}
}
flag = 0;
}
// m_rf_open(CHANNEL, RXADDRESS, PACKET_LENGTH);
// store 10 most recent position readings
for (i=0; i < 11; i++)
{
current_location(position);
x_pos[i] = position[0];
y_pos[i] = position[1];
theta[i] = position[2];
}
// filter position
filter(x_pos,y_pos,theta);
// current position
x_curr = x_pos[11];
y_curr = y_pos[11];
theta_curr = theta[11];
if (x_curr > 0)
{
m_red(ON);
}
else if (x_curr<0)
{
m_red(OFF);
}
if (y_curr > 0)
{
m_green(ON);
}
else if (y_curr<0)
{
m_green(OFF);
}
// send[0] = x_curr;//(char) x_curr;
// send[1] = y_curr;//(char) y_curr;
// send[2] = theta_curr;//(char) theta_curr;
// status = m_rf_send(TXaddress, send, PACKET_LENGTH);
// if (status){m_red(TOGGLE)};
// m_usb_tx_string("X Pos: ");
// m_usb_tx_int(x);
// m_usb_tx_string(" ");
// m_usb_tx_string("Y POSITION: ");
// m_usb_tx_int(y);
// m_usb_tx_string(" ");
// m_usb_tx_string("ANGLE: ");
// m_usb_tx_int(theta1);
// m_usb_tx_string(" ");
// m_usb_tx_string("\n");
a = 2;
// COMM TEST
if (a == 1)
{
// ADD CODE
// m_red(ON);
if (side == 1)
{
set(PORTC,7);
}
else {
set(PORTB,7);
}
}
// PLAY GAME
else if (a == 2) {
// m_red(OFF);
//int hasPuck = follow_puck();
//hasPuck = 1;
// goalie(x_curr, y_curr, theta_curr, side);
if(hasPuck)
{
//score_goal(x_curr, y_curr, theta_curr, side);
//turn_robot(0,0,0,0);
}
}
// GOAL R
else if (a == 3)
{
//side = 1;
//m_green(TOGGLE);
}
// GOAL B
else if ( a == 4)
{
//side = -1;
//m_green(TOGGLE);
}
else if(a == 5)
{
turn_robot(0,0,0,0);
// m_red(ON);
}
// PAUSE, HALF TIME, GAME OVER
if (a == 6 || a == 7)
{
// STOP ROBOT
turn_robot(0,0,0,0);
m_red(ON);
}
else
{}
// // COMM PROTOCOL INSTRUCTIONS
// m_usb_tx_string("STATE: ");
// m_usb_tx_int(a);
// m_usb_tx_string(" ");
// m_usb_tx_string("FLAG: ");
// m_usb_tx_int(flag);
// m_usb_tx_string(" ");
// m_usb_tx_string("TESTING: ");
// m_usb_tx_int(testing);
// m_usb_tx_string(" ");
// m_usb_tx_string("\n");
}
}
int main(void)
{
m_clockdivide(3); //2MHz
//Timer
clear(TCCR1B,CS12);
set(TCCR1B,CS11);
clear(TCCR1B,CS10); //8 precale timer
set(TCCR1B,WGM13);
set(TCCR1B,WGM12);
set(TCCR1A,WGM11);
set(TCCR1A,WGM10);
set(TCCR1A,COM1B1);
clear(TCCR1A,COM1B0);
m_usb_init();
m_imu_init(0,0);
OCR1A = 250;
//read=1;
while(1){
read = m_imu_raw(data);
ax = data[0];
ay = data[1];
az = data[2];
gx = data[3];
gy = data[4];
gz = data[5];
gx_filtered=gx_filtered_prev+gx*dt;
gx_filtered_latest = 0.99*(gx_filtered_old + (gx_filtered-gx_filtered_prev));
//gx_previous = gx;
//gx = gx_previous
//ay_filtered = 0.99*ay_filtered + 0.01*ay;
//a = pow((pow(ax,2) + pow(ay,2) + pow(az,2)),0.5);
//g = pow((pow(gx,2) + pow(gy,2) + pow(gz,2)),0.5);
angle = 0.98*(angle+(gx_filtered*dt))+0.02*ay;
// ay_refined = (ay - ay_offset)*ay_scale;
error = target_angle-angle;
az = (kp * error) + (kd*(error-error_last)/dt) + (ki*sum_error*dt);
OCR1B = abs((kp * error) + (kd*(error-error_last)/dt) + (ki*sum_error*dt))*250/15000;
error_last = error;
sum_error = sum_error + error;
move = OCR1B;
if (angle>0) {
m_red(OFF);
//OCR1B = 65535 - (65535/12500)*abs(move);
clear(DDRB,1);
clear(PORTB,1);
set(DDRB,2);
set(PORTB,2);
set(DDRD,4);
set(PORTD,4);
clear(DDRD,6);
clear(PORTD,6);
} else {
m_red(ON);
//OCR1B = 65535 - (65535/12500)*abs(move);
set(DDRB,1);
set(PORTB,1);
clear(DDRB,2);
clear(PORTB,2);
set(DDRD,6);
set(PORTD,6);
clear(DDRD,4);
clear(PORTD,4);
}
m_usb_tx_string("error\t");
m_usb_tx_int(error);
// m_usb_tx_string("\tax\t");
// m_usb_tx_int(data[0]);
// m_usb_tx_string("\tay\t");
// m_usb_tx_int(data[1]);
// m_usb_tx_string("\taz\t");
// m_usb_tx_int(data[2]);
// m_usb_tx_string("\tgx\t");
// m_usb_tx_int(data[3]);
// m_usb_tx_string("\tgy\t");
// m_usb_tx_int(data[4]);
m_usb_tx_string("\toffset\t");
m_usb_tx_int(gz);
// m_usb_tx_string("\ta\t");
// m_usb_tx_int(a);
// m_usb_tx_string("\tg\t");
// m_usb_tx_int(g);
m_usb_tx_string("\tmove\t");
m_usb_tx_int(move);
m_usb_tx_string("\tgx_filtered\t");
m_usb_tx_int(gx_filtered);
m_usb_tx_string("\n");
}
}
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()
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
{}
}
}
int main(void)
{
m_clockdivide(0);
Timer3_init();//timer for setting delta t for angle value
timer1_init();
m_usb_init();
m_imu_init(2, 3);
sei();
//for motor control-testing
while(1)
{
if (update_angle) {
m_imu_raw(data);
Ax = (float)data[0]*g/accel_scale;
Ay = (float)data[1]*g/accel_scale;
Az = (float)data[2]*g/accel_scale;
Gx = (float)data[3]/gyro_scale;
Gy = (float)data[4]/gyro_scale;
Gz = (float)data[5]/gyro_scale;
// Mx = (float)data[6];
// My = (float)data[7];
// Mz = (float)data[8];
//angle calculation
theta_Ay = (float)Ay*90/g;
angle = alpha*(angle_old + Gx*timestep*5)+ (1-alpha)*theta_Ay;
angle_old = angle;
delta_angle = angle_old + Gx*timestep*5;
integral_angle += angle;
//PID
//output = (Kp*angle) + Kd*(angle - angle_old)*timestep + (Ki*integral_angle*timestep) ;
output = Kp*angle + Kd*Gx*timestep + Ki*integral_angle*timestep;
m_usb_tx_string("\t Ax: ");
m_usb_tx_int(Ax);
m_usb_tx_string("\t Ay: ");
m_usb_tx_int(Ay);
m_usb_tx_string("\t Az: ");
m_usb_tx_int(Az);
m_usb_tx_string("\t Gx: ");
m_usb_tx_int(Gx);
m_usb_tx_string("\t Gy: ");
m_usb_tx_int(Gy);
m_usb_tx_string("\t Gz: ");
m_usb_tx_int(Gz);
m_usb_tx_string("\t angle_Gx: ");
m_usb_tx_int(angle);
m_usb_tx_string("\t output: ");
m_usb_tx_int(output);
m_usb_tx_string("\n");
update_angle = 0;
}
//PID Control
//
//pwm , sensor , whell rotation spd, fbk loop, pwm generate;
//pseudo code to control motor
// initialise timer for motor here, and set OCR1A,OCR1B,OCR1C value based on the speed u want to control the bot.
// go slow or fast, angle less, slow, angle more faster
if (angle > base_angle){//>
//move forward
set(DDRB,6);//set Port B pin 6 for output
clear(PORTB,6); //clear Port B pin 6}
clear(DDRB,7);
flag_foward = 1;
flag_back = 0;
}
if (angle < -base_angle){
//move backwards
set(DDRB,7);//set Port B pin 7 for output
clear(PORTB,7); //clear Port B pin 7
clear(DDRB,6);
flag_back = 1;
flag_foward = 0;
}
//timer1_init();//testing fr working
timer_update();
}
}
void init(void)
{
m_red(ON);
m_clockdivide(0);
m_bus_init();
m_usb_init();
m_rf_open(CHANNEL, RXADDRESS, PACKET_LENGTH);
WAIT(5); // wait for user to pull up log for connection
send(0,0,0,0); //USB and wireless initialized
while(!m_usb_isconnected());
m_red(OFF);
//m_usb_tx_string("Waiting for robot to respond...");
send(0,0,0,1); //Waiting for robot to respond
while(!connected)
{
m_rf_send(TXADDRESS, in_buffer, PACKET_LENGTH);
BLINK(100);
}
//m_usb_tx_string("Robot responded. \nRequsting PID variables.\n");
send(0,1,0,2); // Robot responded. Requesting PID variables
BLINK(500);BLINK(500);
// Set up ADC for PID
// Set to external AREF
clear(ADMUX, REFS1);
set(ADMUX, REFS0);
// Set ADC prescaler
set(ADCSRA, ADPS2);
set(ADCSRA, ADPS1);
set(ADCSRA, ADPS0);
set(DIDR0, ADC4D);
set(DIDR0, ADC5D);
set(DIDR0, ADC6D);
sei();
set(ADCSRA, ADIE);
clear(ADCSRA, ADATE);
clear(ADCSRA, ADEN);
// MUX selection
clear(ADCSRB, MUX5);
set(ADMUX, MUX2);
clear(ADMUX, MUX1);
clear(ADMUX, MUX0);
// enables the system
set(ADCSRA, ADEN);
// starts the conversion
set(ADCSRA, ADSC);
m_red(OFF);
m_green(OFF);
}
/* Setup USB */
void usb_enable(void)
{
m_usb_init();
while(!m_usb_isconnected());
}
int main(void)
{
init(); // Run Timer Initializacion Code
m_bus_init();
m_usb_init(); // Initialize the USB connection.
m_rf_open(1, 0x15, 3);
set(DDRB, 0);
set(DDRB, 1);
set(DDRB, 2);
set(DDRB, 3);
set(DDRB, 7);
set(DDRD, 3);
set(DDRD, 4);
set(DDRD, 5);
set(DDRD, 6);
set(DDRD, 7);
//while(!m_usb_isconnected()); // wait for a connection
while(1){
if(m_usb_rx_available())
{
input = m_usb_rx_char();
//m_usb_tx_int(input);
m_usb_tx_string(" ");
if (input == 119) {
OCR3A = OCR3A - 1;
}
//if (input == 115) {
// OCR3A = OCR3A + 1;
//}
//if (input == 105) {
// OCR0A = OCR0A - 1;
//}
if (input == 107) {
OCR0A = OCR0A + 1;
}
if (input == 105) {
m_red(TOGGLE);
//des_freq = 100;
OCR3A = (float)62500/des_freq;
duration = 100;
flag_2 = 1;
flag = 0;
;
}
if (input == 115) {
des_freq += 10;
}
m_usb_tx_int(des_freq);
}
if (flag) {
m_red(TOGGLE);
m_rf_read(commands,3);
des_freq = *(int*)&commands[0];
des_freq /= 10;
// Measured: 130 is 60 Hz. 20 is 370 Hz. THIS IS INT.
OCR3A = 130 - (float)(des_freq - 60)/(float)2.8;
duration = commands[2];
flag_2 = 1;
flag = 0;
}
// Max Sound duration is 255 centiseconds, which is 2.55 sec.
if (flag_2) {
// TCNT0 = 0;
conta = 0;
m_green(ON);
set(DDRB, 6);
clear(PORTD, 4);
flag_2 = 0;
}
if (conta == (4*duration)) {
m_green(OFF);
clear(DDRB, 6);
clear(PORTB, 6);
set(PORTD, 4);
}
//if (TCNT0 == OCR0A) {
if (check(TIFR0, OCF0A)){
conta += 1;
set(TIFR0,OCF0A);
}
/*
if (check(ADCSRA,ADIF)) {
set(ADCSRA, ADIF);
if (ADC>100) {
set(PORTB, 0);
}
else{
clear(PORTB, 0);
}
if (ADC>200) {
set(PORTB, 1);
}
else{
clear(PORTB, 1);
}
if (ADC>300) {
set(PORTB, 2);
}
else{
clear(PORTB, 2);
}
if (ADC>400) {
set(PORTB, 3);
}
else{
clear(PORTB, 3);
}
if (ADC>500) {
set(PORTB, 7);
}
else{
clear(PORTB, 7);
}
if (ADC>600) {
set(PORTD, 3);
}
else{
clear(PORTD, 3);
}
if (ADC>700) {
set(PORTD, 4);
}
else{
clear(PORTD, 4);
}
if (ADC>800) {
set(PORTD, 5);
}
else{
clear(PORTD, 5);
}
if (ADC>900) {
set(PORTD, 6);
}
else{
clear(PORTD, 6);
}
if (ADC>1000) {
set(PORTD, 7);
}
else{
clear(PORTD, 7);
}
}
*/
//m_usb_tx_string("\r"); // Carriage Return
//m_usb_tx_uint(ADC);
//m_usb_tx_string("\r"); // Carriage Return
//m_usb_tx_hex(commands[0]);
//m_usb_tx_uint(des_freq);
//m_usb_tx_string(" ");
//m_usb_tx_char(commands[2]);
//m_usb_tx_string(" OCR3A: ");
//m_usb_tx_uint(OCR3A);
//m_usb_tx_string(" ");
// Measured: 0x81 (129, we can use 130) is 60 Hz. 46 (70) is 110 Hz.
// I get values from 60 to 110
// Do only every time the Timer Overflows
//while(!check(TIFR3, OCF3A));
if (check(TIFR3, OCF3A)) {
if(indx<255){
indx += 1;
}
else{
indx = 0;
}
//Define new value for Duty Cycle According to Sine Table
OCR1B = sinewave_data[indx];
set(TIFR3, OCF3A); //Clear Flag
}
// Display information for the SCREEN.
/*
m_usb_tx_string("ADC Input Value: "); //
m_usb_tx_uint(ADC); //transmit over USB
m_usb_tx_string(" DIP Switch Inputs:"); //
//m_usb_tx_uint(check(PIND,4));
//m_usb_tx_string(" "); m_usb_tx_uint(check(PIND,5));
//m_usb_tx_string(" "); m_usb_tx_uint(check(PIND,6));
//m_usb_tx_string(" "); m_usb_tx_uint(check(PIND,7));
m_usb_tx_string(" "); m_usb_tx_uint((check(PIND, 4) * 1 + check(PIND, 5) * 2 + check(PIND, 6) * 4 + check(PIND, 7) * 8 + 1));
m_usb_tx_string(" Duty Cycle "); m_usb_tx_uint(((OCR1A - OCR1B)/OCR1A)*100);
m_usb_tx_string("\r"); // Carriage Return
//m_usb_tx_string("\n"); // Enter
//m_usb_tx_string("\f"); // Form Feed
//m_usb_tx_string("\f"); // Tab
//m_usb_tx_string("\v"); // Vertical Tab
*/
}
return 0; /* never reached */
}