|
楼主 |
发表于 2012-5-21 23:45:36
|
显示全部楼层
完整版,用串口向PC机发送信息:- const int SS495_PIN = A0; // Analog input pin that the SS495 is attached to
- const int PWM_PIN=3; // Pins 3 and 11 are connected to Timer 2.
- const double SampleTime=0.5; //ms
- const int PWM_BIAS=128;
- int Setpoint=850;
- double Kp=1, Ki=0.01, Kd=90;
- double piterm,diterm, iiterm=0;
- int input,output,error;
- unsigned long now,timeChange;
- void setupCoilPWM()
- {
- // Setup the timer 2 as Phase Correct PWM, 3921 Hz.
- pinMode(3, OUTPUT);
- // Timer 2 register: WGM20 sets PWM phase correct mode, COM2x1 sets the PWM out to channels A and B.
- TCCR2A = 0;
- TCCR2A = _BV(COM2A1) | _BV(COM2B1) | _BV(WGM20);
- // Set the prescaler to 8, the PWM freq is 16MHz/255/2/<prescaler>
- TCCR2B = 0;
- TCCR2B = _BV(CS21);
- }
- void setupControlTimer()
- {
- //set timer1 in CTC mode.
- //"ATmega4_88_168_328_DataSheet" , section 16.9.2 Clear Timer on Compare Match (CTC) Mode
- cli(); // disable global interrupts
- TCCR1A = 0; // set entire TCCR1A register to 0
- TCCR1B = 0; // same for TCCR1B
- // turn on CTC mode:
- TCCR1B |= (1 << WGM12);
- // Set CS12 bits for 256 prescaler:
- TCCR1B |= (1 << CS12);
- // set compare match register to desired timer count:
- OCR1A = (SampleTime/1000)*(16000000/256);
- // enable timer compare interrupt:
- TIMSK1 |= (1 << OCIE1A);
- sei(); // enable global interrupts
- }
- void writeCoilPWM(uint8_t pin, int val)
- {
- OCR2B = val;
- }
- ISR(TIMER1_COMPA_vect)
- {
- PID_Compute();
- }
- void PID_Compute( )
- {
- static int lastError=0;
- static unsigned long int lastTime=0;
- now = micros();
- timeChange = (now - lastTime);
- if(timeChange>=SampleTime)
- {
- lastTime = now;
- input=read_input();
- error = -(Setpoint - input);
- iiterm+= (Ki * error);
- iiterm=constrain(iiterm,-255,255);
- piterm=Kp * error;
- diterm=Kd * (error - lastError)/SampleTime;
- output = PWM_BIAS+(piterm + diterm);
- output=constrain(output,0,255);
- writeCoilPWM(PWM_PIN, output);
- /*Remember some variables for next time*/
- lastError = error;
- }
- }
- int read_input()
- {
- static int last=0;
- int r;
- r=analogRead(SS495_PIN);
- if(abs(r-last)<4)
- r=last;
- else
- last=r;
- return r;
- }
- void setup() {
- setupCoilPWM();
- setupControlTimer();
- // initialize serial communications at 9600 bps:
- Serial.begin(115200);
- }
- void loop() {
- // read the analog in value:
- // User commands.
- // if( Serial.available() )
- // {
- // processCommand( Serial.read() );
- // }
- // print the results to the serial monitor:
- char str[50];
- sprintf(str,"%ld, %03d, %04d, %+04d, %+04d, %+04d, %04d",now/1000, input,error,piterm,diterm,(int)iiterm,output);
- Serial.println(str);
- delay(20);
- }
复制代码
PC机监控程序。Matlab,能够自动滚屏 - s1=serial('COM4','Baudrate',115200);
- fopen(s1);
- x=0;time=0;input=0;error=0;piterm=0;diterm=0;iiterm=0;pwm=0;
- str='';
- sen=0;
- try % use try catch to ensure fclose
- figure(1)
- hErr=plot(x,error);
- %title('Dados de LDR Aquisitados');
- hold all
- hP=plot(x,piterm);
- hD=plot(x,diterm);
- hPWM=plot(x,pwm);
- j=1;
- while(j<2000)
- str=fscanf(s1);
- sen=str2num(str);
- if length(sen)<7
- continue
- end
-
- x(j)=j
- time(j)=sen(1);
- input(j)=sen(2);
- error(j)=sen(3);
- piterm(j)=sen(4);
- diterm(j)=sen(5);
- iiterm(j)=sen(6);
- pwm(j)=sen(7);
-
- low=j-200;
- if(low<1)
- low=1;
- end
- up=j;
-
- x_1=x(low:up);
- error_1=error(low:up);
- piterm_1=piterm(low:up);
- diterm_1=diterm(low:up);
- pwm_1=pwm(low:up);
- set(hErr,'XData',x_1,'YData',error_1)
- set(hPWM,'XData',x_1,'YData',pwm_1)
- if(up<200)
- up=200;
- end
- axis([low up -500 1010]);
- drawnow;
- j=j+1;
- end;
- figure(2)
- plot(time,input);
- hold all
- plot(time,error);
- plot(time,piterm);
- plot(time,diterm);
- plot(time,pwm);
- legend('input','error','pIterm','dIterm','pwm');
- fclose(s1);
- delete(s1);
- clear s1;
- catch exception
- fclose(s1); % always, always want to close s1
- throw (exception);
- end
复制代码 |
|