// -------------------------------------------------------------- // MAIN // -------------------------------------------------------------- int main(void) { // Initialize the M2 init(); // Enable global interrupts sei(); /* Main Loop... Runs forever.. */ while (true) { // Whenever a package is received if (flag_rx == 1){ flag_rx = 0; m_rf_read(buffer,PACKET_LENGTH); // Pull the packet //Send through USB the received packet if (USB){ m_usb_tx_hex(buffer[0]); m_usb_tx_string("\n"); } } } return 0; // If for some reason it reaches here exit }
/* Recieve Wireless Data */ void wireless_recieve(void) { m_rf_read(buffer,PACKET_LENGTH); // Read RF Signal state = buffer[0]; update_game_state(); }
int main(void) { //original data int Data[9]={0}; int RealinputACC = 0; int RealinputGyr = 0; //variables of flags int switchNum = 0; int paraDeter = 0; //temporary variables for test int tmp=0; int getValue=0; //variables for PID control int output; int angleHis1=0,angleHis2=0; int inputK=0,inputI=0,inputD=0; int Output=0; int OutputHis1=2000; int OutputHis2=2000; int KpIni=KPINI,KiIni=KIINI,KdIni=KDINI; float Kp = 1; float Ki = 0; float Kd = 0.5; int AngleCali = 0; int AngleActual=0; // Parameters for speed control int speed = 0; // PID variables for speed controller int speedHis1 = 0, speedHis2 = 0; int speedI = 0, speedD = 0; int outputSpeed =0; systemInitial(); // send back comfirm information to prove that wireless has been connected char haha[5]="Hello"; wireless_string(haha,5); wireless_char('\n'); clear(DDRB,0); clear(PORTB,0); // using wireless to change parameters of PID controller, and if DIP switch 1 is ON, this can be skipped if (!check(PINB,0)) { while(1) { //m_green(ON); int success; success = m_rf_read(buffer, packLength); if ((success==1)&&(buffer[0]>=45)) { m_green(ON); ConvertFinishFlag=1; } else{ m_green(OFF); } if (ConvertFinishFlag==1) { if (switchNum==0) { // send back current value of parameter and determine which one should be changed wireless_string(buffer,packLength); wireless_char(':'); if ((buffer[0]=='k')&&(buffer[1]=='p')) { paraDeter = 1; wireless_int(KpIni); } if ((buffer[0]=='k')&&(buffer[1]=='i')) { paraDeter = 2; wireless_int(KiIni); } if ((buffer[0]=='k')&&(buffer[1]=='d')) { paraDeter = 3; wireless_int(KdIni); } m_wait(50); wireless_char('\n'); switchNum++; }else{ // change the parameter, and if the input is not a value, it will send back "SayAgain" and wait untill the input is a value getValue = atoi(buffer); if ((getValue==0)&&(buffer[0]!='0')) { char Sorry1[8] = "SayAgain"; wireless_string(Sorry1,8); m_wait(50); wireless_char('\n'); m_wait(50); switchNum=1; }else{ wireless_string(buffer,packLength); m_wait(50); wireless_char('\n'); m_wait(50); switchNum=0; switch (paraDeter) { case 1:KpIni=getValue;break; case 2:KiIni=getValue;break; case 3:KdIni=getValue;break; } } } // when the input is "start", the robot will start if ((buffer[0]=='s')&&(buffer[1]=='t')&&(buffer[2]=='a')&&(buffer[3]=='r')&&(buffer[4]=='t')) { break; } ConvertFinishFlag=0; memset(buffer,0,packLength); m_wait(100); } } } m_green(OFF); // Initializing IMU while(!m_imu_init(1,0)); int AngleHis=0; // calibrate the balance angle value, the code here is trying to find out offset of the angle for (int numpoint=0;numpoint<499;numpoint++) { if(m_imu_raw(Data)) { RealinputACC = ACCPART*(Data[1]-ACOFFSET); RealinputGyr = GYRPART*(GYOFFSET-Data[3]); Kalman_Filter(RealinputACC,RealinputGyr); } AngleHis+=angle; } AngleCali = AngleHis/500; // Enable timer interrupt and global interrupt set(TIMSK3 , OCIE3A); // Initializing the pin change interrupt which will capture the phase of the signal input of the encoder set(PCMSK0 , PCINT4); set(PCICR , PCIE0); clear(DDRB,4); clear(DDRB,5); sei(); // Initializing all the parameters will be used in the PID control of the speed ( or we could say 'displacement') Kp = (float)KpIni/1000; Ki = (float)KiIni/1000; Kd = (float)KdIni/1000; float Kps, Kds, Kis; int OutputSpeedActual = 0; Kps = 4; Kds = 5; Kis = 0.5; while(1) { if (Timer3Flag==1) //Here is the control loop, all the data sample process and output should be here { cli(); //Try to reduce the possibility of changing the data read from IIC, cause multiple device will use the same line and there also different interrupt in the program // read data from IMU if(m_imu_raw(Data)) { m_red(TOGGLE); } // make the input value get rid of the offset RealinputACC = ACCPART*(Data[1]-ACOFFSET); //The acceleration input without offset RealinputGyr = GYRPART*(Data[3]-GYOFFSET); //The anglar velocity input without offset Kalman_Filter(RealinputACC,RealinputGyr); //Using the Kalman Filter to get the reliable output of the angle // read the changed parameter, because the wireless cannot send float, so change it to float here Kp = (float)KpIni/1000; Ki = (float)KiIni/1000; Kd = (float)KdIni/1000; // calibrate the angle value by using the balance offset of the angle AngleActual = angle-AngleCali; inputK = AngleActual-angleHis1; inputI +=AngleActual; // PID controller, which you will find that Ki always be zero Output = Kp*AngleActual + Ki*inputI + Kd*inputK; // dead region, which means the value in this region won't give the wheels a speed, so remove it from output to make the output speed linear with the input angle if (AngleActual>0) { OutputHis1 =1823-Output; } if (AngleActual<0) { OutputHis1 =2193-Output; } if (AngleActual==0) { if (angleHis1>0) { OutputHis1 =1823; } if (angleHis1<0) { OutputHis1 =2193; } if (angleHis1=0) { OutputHis1 =OutputHis1; } } if (AngleActual>0) { OutputHis2 =1857-Output; } if (AngleActual<0) { OutputHis2 =2148-Output; } if (AngleActual==0) { if (angleHis1>0) { OutputHis2 =1857; } if (angleHis1<0) { OutputHis2 =2148; } if (angleHis1=0) { OutputHis2 =OutputHis2; } } // Try to check whether the output value in the limitation if (OutputHis1>4000) //Detect the limitation of the output value { OutputHis1=4000; }else{ if (OutputHis1<3) { OutputHis1=3; } } if (OutputHis2>4000) //Detect the limitation of the output value { OutputHis2=4000; }else{ if (OutputHis2<3) { OutputHis2=3; } } // set the output duty cycle OCR1B = OutputHis1; // for B6 OCR1C = OutputHis2; // for B7 angleHis2 = angleHis1; //record value of the angle which will be used in PID controller(differential) angleHis1 = AngleActual;//record value of the angle which will be used in PID controller(differential) Timer3Flag=0; } if (Timer3Flag2==INTERRUPT1S) //this condition used to send the wireless data or usb data, cause the frequency here is below 10Hz { cli(); // same concern with above speed += numPulse; //red the the value of holes (net value, which means one direction is positive and the other is negative, here is just a summation) speedI += speed; speedD = speed - speedHis1; // PID controller of the speed(displacement actually) outputSpeed = Kps*speed + Kis*speedI + Kds*speedD; //if (speed>0) //{ //OutputSpeedActual =1800-outputSpeed; //} //if (speed<0) //{ //OutputSpeedActual =2200-outputSpeed; //} //if (speed==0) //{ //if (speed>0) //{ //OutputSpeedActual =1800; //} //if (speed<0) //{ //OutputSpeedActual =2200; //} //if (speed=0) //{ //OutputSpeedActual =OutputSpeedActual; //} //} // For stability concern, I decide to limit this value, which means the maximum change of duty cycle is 500/4000 = 12.5% if (outputSpeed>500) { outputSpeed=500; } if (outputSpeed<-500) { outputSpeed=-500; } OCR1B = OutputHis1+outputSpeed; OCR1C = OutputHis2+outputSpeed; speedHis2 = speedHis1; speedHis1 = speed; // all the wireless receiving and sending codes should be wrote here wireless_int(RealinputACC); // send back value from IMU of acceleration of Y wireless_char('\t'); wireless_int(RealinputGyr); // send back value from IMU of angular speed around X wireless_char('\t'); wireless_int(AngleActual); // send back the current angle value wireless_char('\n'); m_green(TOGGLE); Timer3Flag2=0; // clear the counter numPulse = 0; sei(); } //if (Timer3Flag3==INTERRUPT10MS) //{ //} } }
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 */ }
void transmitEdsCode(){ char haha[5]="Hello"; while(1) { int success; success = m_rf_read(buffer, packLength); if ((success==1)&&(buffer[0]>=45)) { m_green(ON); ConvertFinishFlag=1; } else{ m_green(OFF); } if (ConvertFinishFlag==1) { if (switchNum==0) { // send back current value of parameter and determine which one should be changed wireless_string(buffer,packLength); wireless_char(':'); if ((buffer[0]=='k')&&(buffer[1]=='p')) { paraDeter = 1; wireless_int(KpIni); } if ((buffer[0]=='k')&&(buffer[1]=='i')) { paraDeter = 2; wireless_int(KiIni); } if ((buffer[0]=='k')&&(buffer[1]=='d')) { paraDeter = 3; wireless_int(KdIni); } m_wait(50); wireless_char('\n'); switchNum++; }else{ // change the parameter, and if the input is not a value, it will send back "SayAgain" and wait untill the input is a value getValue = atoi(buffer); if ((getValue==0)&&(buffer[0]!='0')) { char Sorry1[8] = "SayAgain"; wireless_string(Sorry1,8); m_wait(50); wireless_char('\n'); m_wait(50); switchNum=1; }else{ wireless_string(buffer,packLength); m_wait(50); wireless_char('\n'); m_wait(50); switchNum=0; switch (paraDeter) { case 1:KpIni=getValue;break; case 2:KiIni=getValue;break; case 3:KdIni=getValue;break; } } } // when the input is "start", the robot will start if ((buffer[0]=='s') &&(buffer[1]=='t') &&(buffer[2]=='a') &&(buffer[3]=='r') &&(buffer[4]=='t')) { break; } ConvertFinishFlag=0; memset(buffer,0,packLength); m_wait(100); } } }
//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"); } }