NXT Development TutorialPart 2: Sensor/ Motor Collaboration

NTHU CS Freshman CampShu-Ting Wang

Download Links

• http://nmsl.cs.nthu.edu.tw/dropbox/CScamp_code.zip

• http://nmsl.cs.nthu.edu.tw/dropbox/CSCamp_Day2.pptx

Outline

• A Car class• Ultrasonic tracker

– Naive Threshold– Method based Threshold– Proportional Control

• Light sensor based line follower– Naïve line follower– PID controlled line follower

Outline

• A Car class• Ultrasonic tracker

– Naive Threshold– Method based Threshold– Proportional Control

• Light sensor based line follower– Naïve line follower– PID controlled line follower

Recall: Dog

• Features:– Weight– Height– Age– Is Healthy– Favorite Sports

• Behaviors:– Run– Bark– Bite– Eat– Sleep

Recall: Class Dog in Java

public class Dog {…...Features …...

public Dog(int Weight, float height, int Age) {…….How to create a dog based on these information

} public void Bark(){

…….How to bark as a dog……. } public void Eat(){

…….How to eat……. }}

Recall: How an Object Pops Out?

• Use a specific term: new• We new a dog out based on our definition in

class dog

Dog mydog= new Dog(5,1.8,3);mydog.Eat();Mydog.Bark();

Lab 1: Write a Car Class (1/2)

public class Car { // Commands for the motors private final static int forward = 1, backward = 2, stop = 3; private static MotorPort leftMotor = MotorPort.C; private static MotorPort rightMotor= MotorPort.B;

private Car(){ } }

Lab 1: Write a Car Class (2/2)

• We need three methods:– public static void stop()– public static void forward()– public static void backward()

Outline

• A Car class• Ultrasonic tracker

– Naive Threshold– Method based Threshold– Proportional Control

• Light sensor based line follower– Naïve line follower– PID controlled line follower

While()

while(condition){….something…

} • Keep doing something until the condition is not true• For example, keep walking until you see Delta

building while (I don’t see Delta building){ dog.walk(); }

If() if(condition){

….something A… } else

….something B… } • Do something if the condition is true• For example if (I am hungry){ I go to find something to eat } else{ play LOL }

Recall: Ultrasonic Sensor

• Get the distance from your hand

public static void main(String[] args) throws Exception {UltrasonicSensor Ultrasonic =

new UltrasonicSensor(SensorPort.S1);

while(!Button.ESCAPE.isDown()) {LCD.clear(3,0,0);LCD.drawInt(Ultrasonic.getDistance(),3,0,0);

} }

Lab 2: Naïve Threshold Tracker

• Implement a control loop which based on the measured distance controls the movements of the car

• If the measured distance to obstacles/wall are over 40, the car continued straight ahead.

• If not, the car turning to the left• Modify the run() method in Tracker.java

Lab3: isCrashed() ?• Your car got stuck in chair legs during the

rotation ?• Made an isCrashed method whose task is to

check whether the car is stuck based on repeated distance measurements similar to each other.

Proportional Control (1/2)

• The power to the car motors is determined by the measured error

• Error in this case is the difference between the measured distance to the object and the desired distance to the object:

error = distance - desiredDistance;

Proportional Control (2/2)

• The power is proportional to the measured error by multiplying the error with a constant (gain).

power = (int) (gain * error);

Lab 4

• The power is always in between two extremes which are the minimum (set to 60) and maximum ( set to 100) speed

• Modify the run() method in Tracker.java• If error>0, we move forward• Otherwise, we move backward

public void run(){ while ( running ) { distance = us.getDistance(); if ( distance != 255){ error = distance -threshold; power = (int)(Pgain * error); if ( error > 0 ) {

……………}

else {…………….}

Delay.msDelay(200); } } }

Outline

• A Car class• Ultrasonic tracker

– Naive Threshold– Method based Threshold– Proportional Control

• Light sensor based line follower– Naïve line follower– PID controlled line follower

Lab 5: Straight Line Follower

• Fix a bug that determines the light sensor input is white or black

• How to control the two motors?

Overview of PID Controller

• The Setpoint (SP) is the value that we want the process to be

• If the SP and the Process Variable PV are the same – then the controller is a very happy little box

Usage Scenario

• We want the AC in our classroom to achieve a steady temperature of as close to 22°C as possible

• What should we do?

Another Example

• How you drive a car on windy freeway ?• Basically, you are a blackbox controller

The Blackbox: PID Controller

PID?

• Proportional: multiplies the Error by the Gain (Kp)

• Integral: the sum of instantaneous errors over time

• Derivative: a mathematical term meaning rate-of-change. It predicts future errors

Effects of increasing a parameter independently

Parameter Rise time Overshoot Settling time Steady-state error Stability

Decrease Increase Small change Decrease Degrade

Decrease Increase Increase Eliminate DegradeMinor change Decrease Decrease No effect in

theoryImprove if small

                                      

Principle ?

• React faster: Kd• Move steadily: Ki• Try different parameters to make your car run

faster and more steadily on the given track

How to Tune the Parameters

……PIDController pid = new PIDController (threshold, 10);pid.setPIDParam(PIDController.PID_KP, 12.0f); pid.setPIDParam(PIDController.PID_KI, 0.009f); pid.setPIDParam(PIDController.PID_KD, 300.0f);……

PID Controller in Java (1/3)

public int doPID(int processVariable){ int outputMV; this.PV = processVariable; error = setpoint - processVariable; if(Math.abs(error)<=deadband){

error=0; } if (!disableIntegral) { integral += Ki * error * dt; }

PID Controller in Java (2/3)

if (integralLimited){ if (integral>integralHighLimit){

integral = integralHighLimit; } if (integral<integralLowLimit) {

integral = integralLowLimit; }}derivative = ((float)(error - previous_error))/dt;outputMV = (int)(Kp*error + integral + Kd*derivative);

PID Controller in Java (3/3)

if (outputMV>highLimit){ outputMV=highLimit;}if (outputMV<lowLimit){ outputMV=lowLimit;}previous_error = error;outputMV=rampOut(outputMV);

Lab 6:public static void PIDControl(PIDController pid){

pid.setPIDParam(PIDController.PID_KP, 12.0f); pid.setPIDParam(PIDController.PID_KI, 0.009f); pid.setPIDParam(PIDController.PID_KD, 300.0f); pid.setPIDParam(PIDController.PID_LIMITHIGH, 200.0f);

pid.setPIDParam(PIDController.PID_LIMITLOW, -200.0f);pid.setPIDParam(PIDController.PID_I_LIMITHIGH, 0.0f);pid.setPIDParam(PIDController.PID_I_LIMITLOW, 0.0f);pid.setPIDParam(PIDController.PID_DEADBAND, 1);

}

• Insert this function inti BlakcWhiteSensor.java• We want to see how these parameters work with PID

controller

Reference

• leJOS official tutorial • leJOS API documentation