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;
}
}
}
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
}
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
}
}
}
int main(void)
{
m_clockdivide(0);
m_bus_init();
yes = m_port_init(ADDR);
int flag = 0;
set(DDRD,7);
m_port_set(ADDR,DDRH,7);
while(1)
{
if (yes){
clear(PORTD,7);
}
else {
set(PORTD,7);
}
m_wait(500);
if (flag == 0) {
m_green(TOGGLE);
m_port_set(ADDR,PORTH,7);
set(PORTD,7);
flag = 1;
} else {
m_port_clear(ADDR,PORTH,7);
clear(PORTD,7);
flag = 0;
}
}
}
int main(void)
{
m_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');
}
}
// --------------------------------------------------------------
// 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);
setup_pins();
if ( RF_debug) {setupUSB();}
setup_timer_1();
setup_timer_3();
m_bus_init();
m_rf_open(chan,RX_add,p_length);
int timer_3_cnt = 0;
//sei();
set_motors(0,0);
while (1){
if (check(TIFR3,OCF3A)){
set(TIFR3, OCF3A);
timer_3_cnt++;
if ( timer_3_cnt % 10 ==0 ) {
timer_3_cnt=0;
m_green(TOGGLE);
}
if ( timer_3_cnt == 1000){ timer_3_cnt=0;}
}
if(new){
switch ( receive_buffer[11] ) {
case Single_Joy: single_joystick(); break;
case Double_Joy: double_joystick(); break;
case Tank_Mode: tank_driving(); break;
case Mario_Kart: Mario_Drive(); break;
default : set_motors(0,0); m_green(2);
}
if(RF_debug){ debug_rf(); }
}
}
}
int main (void)
{
InitPeripherals();
m_clockdivide(0);
TWI_Slave_Initialise(I2C_ADDR<<TWI_ADR_BITS);
sei();
TWI_Start_Transceiver();
ADC_Init ADC_test;
ADC_test.ADC_CHANNEL = ADC0;
ADC_test.ADC_REF = VCC;
ADC_test.ADC_PRESCALAR = 64;
ADC_test.ADC_MODE = FREE_RUN;
adc_init(ADC_test);
adcInterruptEnable;
adcEnable;
adcAutoTriggerEnable;
adcStart;
while(1)
{
process_command();
}
}
void init()
{
m_clockdivide(0);
m_disableJTAG();
movement_init();
sensor_init();
}
int main(void) {
m_disableJTAG();
m_clockdivide(2);
setup_pins();
if (debug_fire|| RF_debug) {setupUSB();}
setup_timer_1();
setup_timer_3();
m_bus_init();
m_rf_open(chan,RX_add,p_length);
int timer_3_cnt = 0;
//sei();
set_motors(0,0);
while (1){
if (check(TIFR3,OCF3A)){
set(TIFR3, OCF3A);
timer_3_cnt++;
if(fired){
since_fired++;
if (debug_fire){
m_usb_tx_string(" its been\t");
m_usb_tx_int(since_fired);
m_usb_tx_string(" milisec\n\r");
}
}
if (since_fired>10){
clear(PORTF,5);
since_fired=0;
fired = false;
if (debug_fire){m_usb_tx_string(" its been 100 sec\n\r");}
}
if ( fire && check(PINB,3)){
fire=false;
fired=true;
since_fired=0;
if (debug_fire){m_usb_tx_string(" portb 3 is high\n\r");}
}
// m_rf_open(chan,RX_add,p_length);
m_green(TOGGLE);
m_rf_init();
if ( timer_3_cnt == 10){
m_red(2);
timer_3_cnt=0;
m_rf_open(chan,RX_add,p_length);
}
}
if(new){ turretDrive();
if(RF_debug){ debug_rf(); }
if ((receive_buffer[0] == 1 || receive_buffer[1]==1) && !fire){FIRE();}
}
//TODO fill in timer code for the firing mechanism
}
}
/* Initialization of Pins and System Clock */
void init(void){
m_clockdivide(0); // Set to 16 MHz
set(DDRB,6); // Set B6 to output
set(DDRB,0); // Set B0 to output
sei(); // Enable global interrupts
}
void systemInitial(void){
m_clockdivide(0);
wirelessinitial();
TimerSetup();
//m_usb_init();
//while(!m_imu_init(1,0));
//sei();
//while(!m_usb_isconnected());
}
int main (void)
{
m_clockdivide(0);
twi_init(I2C_ADDR);
while(1)
{
if (receive_done)
{
process_command();
receive_done = false;
TWI_ACK();
}
}
}
/* Initialization of Pins and System Clock */
void init(void){
m_clockdivide(CLOCK_DIVIDE); // Set to 2 MHz
//Set to Input
clear(DDRD,0); // D0
clear(DDRD,1); // D1
clear(DDRD,2); // D2
//Set to Output
while(!m_imu_init(accel_scale,gyro_scale)); //Initialize IMU
sei(); // Enable global interrupts
}
// 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
}
int main() {
m_red(ON);
m_clockdivide(0);
m_disableJTAG();
m_bus_init();
m_rf_open(1, 0xD1, 10);
m_red(OFF);
char buf[10] = {0x08, 10, 10, 0, 0, 0, 0, 0, 0, 0};
while(1) {
m_green(ON);
m_rf_send(0xDA, buf, 10);
buf[1] += 5;
if(buf[1] >= 220) buf[1] = 10;
m_green(OFF);
m_wait(500);
}
return 0;
}
int main(void){
m_clockdivide(0);
int data[9] = {0,0,0,0,0,0,0,0,0};
m_green(ON);
m_bus_init();
m_usb_init();
while(!m_usb_isconnected());
m_green(OFF);
m_imu_init(accel_scale, gyro_scale);
m_rf_open(CHANNEL, RXADDRESS, PACKET_LENGTH);
while(1) {
while ( !newPacket );
int i,a;
while (i=0; i<PACKET_LENGTH , i+=2 ){
a = *(int*)&packet[i*2+2];
m_usb_tx_int(a);
m_usb_tx_char('\t');
}
m_usb_tx_char('\n');
m_usb_tx_char('\r');
}
}
int main(void)
{
sei(); //enable interrupts
m_clockdivide(0); // 16 MHz
m_bus_init(); // enable mBUS
m_rf_open(CHANNEL,RXADDRESS,PACKET_LENGTH); // configure mRF
//set the clock pre-scaler to make the clock very fast
//set the clock counter
OCR3A = 2500;
//configure pin 6 for output
set(DDRB,6);
//set the mode of timer 1
set(TCCR1B,WGM13);
set(TCCR1B,WGM12);
set(TCCR1A,WGM11);
set(TCCR1A,WGM10);
//clear OCR1B at rollover
set(TCCR1A,COM1B1);
//timer 1 interrupt handle
set(TIMSK1,OCIE1A);
//set the mode of timer 3
set(TCCR3B,WGM32);
//timer 3 interrupt handle
set(TIMSK3,OCIE3A);
while(1)
{
// n=100;
// i=0;
}
}
/* Initialization of Pins and System Clock */
void init(void){
m_clockdivide(0); // Set to 16 MHz
sei(); // Enable global interrupts
}
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);
}
/*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_disableJTAG();
m_red(ON);
m_clockdivide(0); // clock speed 16 MHz
sei(); // enable global interrups
// initialize m_bus communications
// start wireless communications
// and open wii camera
m_bus_init();
m_wii_open();
m_usb_init();
m_rf_open(CHANNEL, RXADDRESS, PACKET_LENGTH);
setup(); // motor setup
ADC_setup();
int position[3]; // array for robot position
int x_curr;
int y_curr;
int theta_curr;
int i;
int a;
int x_pos[11];
int y_pos[11];
int theta[11];
// main LOOP
while(1)
{
// for (i=0; i < 11; i++)
// {
// current_location(position);
// x_pos[i] = position[0];
// y_pos[i] = position[1];
// theta[i] = position[2];
// }
// // calculate positions
// filter(x_pos,y_pos,theta);
// x_curr = x_pos[11];
// y_curr = y_pos[11];
// theta_curr = theta[11];
// Read in values
// ADC0();
// int D6 = ADC;
//
// ADC1();
// int F1 = ADC;
//
// ADC4();
// int F4 = ADC;
//
// ADC5();
// int F5 = ADC;
//
// ADC6();
// int F6 = ADC;
//
// ADC7();
// int F7 = ADC;
//
// ADC8();
// int D7 = ADC;
//
follow_puck();
m_green(TOGGLE);
// // usb prinitng
// m_usb_tx_string("D6: ");
// m_usb_tx_int(D6);
// m_usb_tx_string(" ");
// m_usb_tx_string("F1: ");
// m_usb_tx_int(F1);
// m_usb_tx_string(" ");
// m_usb_tx_string("F4: ");
// m_usb_tx_int(F4);
// m_usb_tx_string(" ");
// m_usb_tx_string("F5: ");
// m_usb_tx_int(F5);
// m_usb_tx_string(" ");
// m_usb_tx_string("F6: ");
// m_usb_tx_int(F6);
// m_usb_tx_string(" ");
// m_usb_tx_string("F7: ");
// m_usb_tx_int(F7);
// m_usb_tx_string(" ");
// m_usb_tx_string("D7: ");
// m_usb_tx_int(D7);
// m_usb_tx_string(" ");
// m_usb_tx_string("\n");
a = state(buffer1);
a = 2;
// COMM TEST
if (a == 1)
{
// ADD CODE
}
// PLAY
if (a == 2) {
}
// PAUSE, HALF TIME, GAME OVER
if (a == 5 || a == 6 || a == 7)
{
// STOP ROBOT
turn_robot(0,0,0,0);
m_red(OFF);
}
else
{}
}
}
void systemInitial(void){
m_clockdivide(0);
wirelessinitial();
TimerSetup();
}
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);
}
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_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)
{
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); // 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);
}