Пример #1
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');
    }
}
Пример #2
0
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)
		//{
		//}				
    }
}
Пример #3
0
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");


}
}
Пример #4
0
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();
		}		
    }
}
Пример #5
0
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();
    }
}
Пример #6
0
/************************************************************
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);
		}
	}
}