void debug_ADC_sums(int timer_3_cnt, long VRightSum,long VLeftSum, long HLeftSum, long HRightSum ){
m_usb_tx_int(timer_3_cnt);
m_usb_tx_string(" me \t");
m_usb_tx_long(VRightSum);
m_usb_tx_char('\t');
m_usb_tx_long(HRightSum);
m_usb_tx_char('\t');
m_usb_tx_long(VLeftSum);
m_usb_tx_char('\t');
m_usb_tx_long(HLeftSum);
m_usb_tx_string("\t\n\r");
}
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 get_command(){
comm_cmd = false;
state_requested = buffer[0];
if (debug_com){
m_usb_tx_string("state requested:\t");
m_usb_tx_int(state_requested);
m_usb_tx_string("\n\r");
}
m_red(OFF);
clear(PORTB,1);
clear(PORTB,2);
clear(PORTB,3);
if(state_requested == commTest){
set(PORTB,1);
}
else if(state_requested == play){
set(PORTB,2);
}
else if(state_requested == goalA){
set(PORTB,1);
set(PORTB,2);
}
else if(state_requested == goalB){
set(PORTB,3);
}
else if(state_requested == pause){
set(PORTB,3);
set(PORTB,1);
}
else if(state_requested == halfTime){
set(PORTB,3);
set(PORTB,2);
}
else if(state_requested == gameOver){
set(PORTB,3);
set(PORTB,2);
set(PORTB,1);
} else {
m_red(ON);
}
}
void select_goal(void)
{
/* Assign Defending goal */
update_position();
float position_buffer[3];
get_position(position_buffer);
m_usb_tx_int((int)position_buffer[0]);
if (position_buffer[0]>0) {
x_goal = -1*GOAL_X_DIST;
goal = BLUE;
//positioning_LED(RED);
} else {
x_goal = GOAL_X_DIST;
goal = RED;
//positioning_LED(BLUE);
}
}
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 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);
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 doUSB(void)
{
m_usb_tx_string("\n");
// if(m_usb_isconnected())
// {
// m_red(ON);
//m_usb_tx_char(97);
// m_usb_tx_int(debugVar);
// m_usb_tx_char(32);
// int i;for (i=0;i<12;i++)
// {
// m_usb_tx_uint(blobMemAddr[i]);
// m_usb_tx_char(32);
// }
if (currentTeam == RED) {
m_usb_tx_string("RED ");
} else if (currentTeam == BLUE) {
m_usb_tx_string("BLUE ");
} else {
m_usb_tx_string("NO TEAM! ");
}
// m_usb_tx_char(40);
// m_usb_tx_int(robotX);
// m_usb_tx_char(44);
// m_usb_tx_int(robotY);
// m_usb_tx_char(41);
m_usb_tx_char(32);
m_usb_tx_char(60);
m_usb_tx_int(angleOfRobot); // (X,Y) <ANGLE
m_usb_tx_char(32);
m_usb_tx_string("DUTY: ");
m_usb_tx_int(MOTOR_TIMER_OCR_R);
m_usb_tx_int(MOTOR_TIMER_OCR_L);
m_usb_tx_int(motorDutyL);
m_usb_tx_char(32);
m_usb_tx_int(motorDutyR);
m_usb_tx_string("ANGLE_TO_ENEMY_GOAL:");
m_usb_tx_int(angleToEnemyGoal);
m_usb_tx_string("\n");
// m_usb_tx_uint(robotY);
// m_usb_tx_int( angleOfRobot);
// }
//****************************
m_usb_tx_string(" ROBOT ANGLE: ");m_usb_tx_int(angleOfRobot);m_usb_tx_char(32);
m_usb_tx_string(" THETA1: ");m_usb_tx_int(angleToRobot);m_usb_tx_char(32);
m_usb_tx_string(" THETA2: ");m_usb_tx_int(theta2);m_usb_tx_char(32);
m_usb_tx_string("ROBOT COORDS: (");m_usb_tx_int(robotX);m_usb_tx_char(32);
m_usb_tx_int(robotY);m_usb_tx_string(")");m_usb_tx_char(32);
}
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
}
}
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();
}
}
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)
{
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 */
}
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 state_play()
{
//F4 for acd conversion
clear(ADCSRA,ADEN);
clear(ADCSRB,MUX5);//setting mux to F4
set(ADMUX,MUX2);
clear(ADMUX,MUX1);
clear(ADMUX,MUX0);
//starting adc for F4
set(ADCSRA,ADEN);
set(ADCSRA,ADSC);
while(!check(ADCSRA,ADIF));
adc_values[1] = ADC;//saving adc value
set(ADCSRA,ADIF);//clearing flag
//F6 for acd conversion
clear(ADCSRA,ADEN);
clear(ADCSRB,MUX5);//setting mux to F6
set(ADMUX,MUX2);
set(ADMUX,MUX1);
clear(ADMUX,MUX0);
//starting adc for F6
set(ADCSRA,ADEN);
set(ADCSRA,ADSC);
while(!check(ADCSRA,ADIF));
adc_values[0] = ADC;//saving adc value
set(ADCSRA,ADIF);//clearing flag
clear(ADCSRA,ADEN);
//D6 for acd conversion
set(ADCSRB,MUX5);//setting mux to D6
clear(ADMUX,MUX2);
clear(ADMUX,MUX1);
set(ADMUX,MUX0);
//starting adc for D6
set(ADCSRA,ADEN);
set(ADCSRA,ADSC);
while(!check(ADCSRA,ADIF));
adc_values[2] = ADC;//saving adc value
set(ADCSRA,ADIF);//clearing flag
//D4 for acd conversion
clear(ADCSRA,ADEN);
set(ADCSRB,MUX5);//setting mux to D4
clear(ADMUX,MUX2);
clear(ADMUX,MUX1);
clear(ADMUX,MUX0);
//starting adc for D4
set(ADCSRA,ADEN);
set(ADCSRA,ADSC);
while(!check(ADCSRA,ADIF));
adc_values[3] = ADC;//saving adc value
set(ADCSRA,ADIF);//clearing flag
puck_status = 0;
if(adc_values[0]>700 && adc_values[0]>adc_values[2] && adc_values[0]>adc_values[3])
puck_status = 2;
if(adc_values[1]>800 && (adc_values[1]-adc_values[2] < -100 || adc_values[1]-adc_values[2] > 100) && adc_values[1]>adc_values[3])
puck_status = 2;
if(adc_values[1]>800 && (adc_values[1]-adc_values[2] > -100 && adc_values[1]-adc_values[2] < 100))
puck_status = 0;
if(adc_values[2]>800 && (adc_values[1]-adc_values[2] < -100 || adc_values[1]-adc_values[2] > 100) && adc_values[2]>adc_values[1])
puck_status = 1;
if(adc_values[3]>800 && adc_values[3]>adc_values[0] && adc_values[3]>adc_values[1])
puck_status = 1;
switch(puck_status){
case 1:
set_left(90);
set_right(50);
m_red(ON);
m_green(ON);
break;
case 2:
set_left(-90);
set_right(-50);
m_red(ON);
m_green(OFF);
break;
case 0:
set_left(0);
set_right(0);
m_red(OFF);
m_green(ON);
break;
}
get_position(blobs, &x, &y, &theta);
if(m_usb_isconnected()){
//print out adc values to screen
/*m_red(TOGGLE);
m_usb_tx_string("Phototrans 1 = ");
m_usb_tx_int((int) adc_values[0]);
m_usb_tx_string("\n");
m_usb_tx_string("Phototrans 2 = ");
m_usb_tx_int((int)adc_values[1]);
m_usb_tx_string("\n");
m_usb_tx_string("Phototrans 3 = ");
m_usb_tx_int((int)adc_values[2]);
m_usb_tx_string("\n");
m_usb_tx_string("Phototrans 4 = ");
m_usb_tx_int((int)adc_values[3]);
m_usb_tx_string("\n");
m_usb_tx_string("puck status = ");
m_usb_tx_int((int)puck_status);
m_usb_tx_string("\n");
m_wait(1000);*/
m_usb_tx_string("X = ");
m_usb_tx_int((int) x-x_center);
m_usb_tx_string("\n");
m_usb_tx_string("Y = ");
m_usb_tx_int((int) y-y_center);
m_usb_tx_string("\n");
m_usb_tx_string("Theta = ");
m_usb_tx_int((int) theta-theta_zero);
m_usb_tx_string("\n");
}
wireless_buffer_f = false;
char inst = wireless_buffer[0];
switch(inst)
{
case 0xA4:
state_pause();
break;
case 0xA1:
state_play();
return;
case 0xA5:
state_detangle();
return;
default:
state_play();
return;
}
}
void reportADC() {
m_red(ON);
m_green(ON);
m_red(OFF);
m_green(OFF);
char rx_buffer; //computer interactions
int index = 0;
int maxval = 0;
float deg = 0.0;
float diff = 0.0;
getADC();
index = 0;
maxval = 0;
//m_green(TOGGLE);
for(int i = 0; i < 8; i++) {
if (ADCarr[i] > maxval) {
index = i;
maxval = ADCarr[i];
}
}
switch (index) {
case 0:
diff = ADCarr[0] - ADCarr[6];
deg = exp(-1.0*fabs(((float)diff))/400.0);
//m_green(ON);
//m_red(OFF);
break;
case 6:
diff = ADCarr[6] - ADCarr[0];
deg = exp(-1.0*fabs(((float)diff))/400.0);
//m_green(OFF);
//m_red(ON);
break;
}
//while(!m_usb_rx_available()); //wait for an indication from the computer
//rx_buffer = m_usb_rx_char(); //grab the computer packet
//m_usb_rx_flush(); //clear buffer
//if(rx_buffer == 1) { //computer wants ir buffer
//write ir buffer as concatenated hex: i.e. f0f1f4f5
m_usb_tx_int(ADCarr[0]);
m_usb_tx_char('\t');
m_usb_tx_int(ADCarr[1]);
m_usb_tx_char('\t');
m_usb_tx_int(ADCarr[2]);
m_usb_tx_char('\t');
m_usb_tx_int(ADCarr[3]);
m_usb_tx_char('\t');
m_usb_tx_int(ADCarr[4]);
m_usb_tx_char('\t');
m_usb_tx_int(ADCarr[5]);
m_usb_tx_char('\t');
m_usb_tx_int(ADCarr[6]);
m_usb_tx_char('\t');
m_usb_tx_int(ADCarr[7]);
m_usb_tx_char('\t');
m_usb_tx_int(index);
m_usb_tx_char('\t');
m_usb_tx_int((int)(deg*100));
m_usb_tx_char('\t');
//}
m_usb_tx_char('\n'); //MATLAB serial command reads 1 line at a time
//}
}
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);
}
}
void measure(int select){
//m_green(TOGGLE);
toggle(PORTB,2);
int* bar;
selectIMU(select);
switch(select){
case 0:
bar = bar0;
break;
case 1:
bar = bar1;
break;
case 2:
bar = bar2;
break;
case 3:
bar = bar3;
break;
case 4:
bar = bar4;
break;
case 5:
bar = bar5;
break;
}
//m_usb_tx_int(select);
//m_usb_tx_string("\t");
micros = 4096 * overflow + (unsigned long)((float)(((unsigned long)(TCNT3H) << 8) | TCNT3L) * 4096 / 65536);
m_usb_tx_ulong(micros);
m_usb_tx_string("\t");
m_imu_accel(raw_data_buffer);
ax = raw_data_buffer[0] - bar[0];
ay = raw_data_buffer[1] - bar[1];
az = raw_data_buffer[2] - bar[2];
m_usb_tx_int(ax);
m_usb_tx_string("\t");
m_usb_tx_int(ay);
m_usb_tx_string("\t");
m_usb_tx_int(az);
m_usb_tx_string("\t");
m_imu_gyro(raw_data_buffer);
gx = raw_data_buffer[0] - bar[3];
gy = raw_data_buffer[1] - bar[4];
gz = raw_data_buffer[2] - bar[5];
m_usb_tx_int(gx);
m_usb_tx_string("\t");
m_usb_tx_int(gy);
m_usb_tx_string("\t");
m_usb_tx_int(gz);
m_usb_tx_string("\t");
m_imu_mag(raw_data_buffer);
mx = raw_data_buffer[0];
my = raw_data_buffer[2];
mz = raw_data_buffer[1];
m_usb_tx_int(mx);
m_usb_tx_string("\t");
m_usb_tx_int(my);
m_usb_tx_string("\t");
m_usb_tx_int(mz);
m_usb_tx_string("\n");
}
/************************************************************
Main Loop
************************************************************/
int main(void)
{
/* Confirm Power */
m_red(ON);
/* Initializations */
init();
usb_enable();
timer3_init();
int gy_previous_reading = 0;
/* Confirm successful initialization(s) */
m_green(ON);
/* Run */
while (1){
if (m_imu_raw(data))
{
m_green(ON);
m_red(OFF);
ax = lowpass(0.85,ax,data[0])+AX_OFFSET;
az = lowpass(0.85,az,data[2])+AZ_OFFSET;
gy = lowpass(ALPHA_LOW,gy,data[4])+GY_OFFSET;
gy = highpass(ALPHA_HIGH,gy,gy_previous_reading,data[4]);
gy_previous_reading = data[4];
/*
m_usb_tx_string("ax= ");
m_usb_tx_int(ax);
m_usb_tx_string(" az=");
m_usb_tx_int(az);
m_usb_tx_string(" gy=");
m_usb_tx_long(gy);
m_usb_tx_string("\n");
*/
int angle = ((float)ax*RAD2DEG)/sqrt(((float)ax*ax+(float)az*az));
if (check(TIFR3,OCF3A)){ //check if timestep has completed
angle += gy*TIMESTEP; //add thetadot*timestep to angle
set(TIFR3,OCF3A); //reset flag
}
m_usb_tx_int(angle);
m_usb_tx_string("\n");
/*
m_usb_tx_string("ax= ");
m_usb_tx_int(data[0]);
m_usb_tx_string(" ay= ");
m_usb_tx_int(data[1]);
m_usb_tx_string(" az= ");
m_usb_tx_int(data[2]);
m_usb_tx_string(" gx= ");
m_usb_tx_int(data[3]);
m_usb_tx_string(" gy= ");
m_usb_tx_int(data[4]);
m_usb_tx_string(" gz= ");
m_usb_tx_int(data[5]);
m_usb_tx_string("\n");
*/
}
else
{
m_green(OFF);
m_red(ON);
}
}
}
