|
发表于 2010-6-7 17:13:44
|
显示全部楼层
NXC的:
- // 取样时间
- #define dt 25
- // 定义颜色传感器
- #define COLORSENSOR SENSOR_2
- task main()
- {
- int error = 0;
- float previous_error = 0;
- float setpoint = 0;
- float actual_position = 0;
- float proportional = 0;
- int integral = 0;
- float derivative = 0;
- float output = 0;
- float left = 0;
- float right = 0;
- // 马达速度
- float speed=50;
- // 开传感器
- SetSensorColorFull(IN_2);
- // 传感器在黑线上方为启示状态
- TextOut(1,LCD_LINE1,"Setpoint");
- setpoint = COLORSENSOR;
- NumOut(50,LCD_LINE1,setpoint);
- // 循环
- while (true)
- {
- // 读取传感器实施数值
- actual_position = COLORSENSOR;
- TextOut(1,LCD_LINE2,"Actual");
- NumOut(50,LCD_LINE2,actual_position);
- // 计算误差值
- error = setpoint - actual_position;
- // Play a sound when the sensor is off the line
- if (error <> 0) PlayTone(TONE_B7, 1);
- // PID控制
- proportional = Kproportional * error;
- integral = integral + error;
- derivative = (error - previous_error) / dt;
- output = proportional + Kintegral * dt * integral + Kderivative * derivative;
- previous_error = error;
- // 左轮输出
- left = speed - output;
-
- // 右轮输出
- right = speed + output;
- // 调整输出溢出
- if (left > 100) left = 100;
- if (left < -100) left = -100;
- if (right > 100) right = 100;
- if (right < -100) right = -100;
- if (left < 0 )
- {
- OnFwd(OUT_A,-left);
- TextOut(1,LCD_LINE4,"Left Rev");
- NumOut(55,LCD_LINE4,-left);
- }
- else
- {
- OnRev(OUT_A,left);
- TextOut(1,LCD_LINE4,"Left Fwd");
- NumOut(55,LCD_LINE4,left);
- }
- if (right < 0 )
- {
- OnFwd(OUT_B,-right);
- TextOut(1,LCD_LINE5,"Right Rev");
- NumOut(55,LCD_LINE5,-right);
- }
- else
- {
- OnRev(OUT_B,right);
- TextOut(1,LCD_LINE5,"Right Fwd");
- NumOut(55,LCD_LINE5,right);
- }
- // 等待取样时间
- Wait(dt);
- }
- }
复制代码 |
|