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');
}
}
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)
{
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) {
//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;
systemInitial();
// send back comfirm information to prove that wireless has been connected
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)) {
transmitEdsCode();
}
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);
set(PCICR,0);
set(PCMSK0,5);
// set(PCMSK0,4);
sei();
while(1)
{
if (Timer3Flag==1){ //Here is the control loop, all the data sample process and output should be here
cli();
// 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 D
angleHis1 = AngleActual;//record value of the angle which will be used in D
Timer3Flag=0;
sei();
}
//*****************************************************************
// Because the holes on our encoder are few, so in the 5 ms period it
// probably won't get any data. So my advice is to write the code for
// encoder here, which period is 100ms, then we may get some points,
// or if we like we can use flag3 and set it to 1000ms to get more
// points, 1s is enough short to get precise position
//*****************************************************************
//condition used to send the wireless data or usb data,
// because the frequency here is below 10Hz
if (Timer3Flag2==INTERRUPT1S) {
cli();
tmp = angle;
// send back value from IMU of acceleration of Y
wireless_int(RealinputACC);
wireless_char('\t');
// send back value from IMU of anglar speed around X
wireless_int(RealinputGyr);
wireless_char('\t');
wireless_int(AngleActual); // send back the current angle value
wireless_char('\n');
m_green(TOGGLE);
Timer3Flag2=0;
sei();
}
}
}
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();
}
}
/************************************************************
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);
}
}
}
