// --------------------------------------------------------------
// 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");
}
}
