#pragma config(Sensor, S2, MS, sensorNone) //Magnetic Field Sensor #pragma config(Sensor, S3, LS, sensorLightActive) //Light Sensor

//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*// /* Project: Basic Line Follower Version: 2.1 Last edited: 28.02.11 How to use: Setup: this code is and its PID values were optimized to best fit the robot I used to test and tune it. So, to use this code optimally, make your robot: has two MindStorms 1.0 wheels, 16 cm apart has its caster 13 cm in front of the middle of the two wheels has a light sensor 6 cm behind the caster, 1 cm above the ground has its light sensor plugged in to port 3 has a stabily mounted magnet in front of the HiTechnic magnetic field sensor has its magnetic field sensor plugged into port 2 When starting the program, position the light sensor above the black line, and, after the beep, above the white surface to the right of it. The robot will start driving after a split second. To stop it, remove the magnet from its stable position (if you'd like for the robot to carry on, place the magnet back EXCACTLY the way it was when the robot started driving! Copyright information: Magnetic field sensor by HiTechnic The driver for the HiTechnic magnetic field sensor was created by Xander Soldaat, aka Mightor (http://www.mightor.wordpress.com) */ #include "drivers/HTMAG-driver.h" task main(){ float offset; //variables float Kp = 2; float Ki = 0.001;

float Kd = 15; float Tp = 60; float error; float integral; float derivative; float lastError; float turn; float powerB; float powerC; int tooLow; int tooHigh; wait10Msec(200); PlaySound(soundBlip); tooLow = SensorValue(LS); //get black light value wait10Msec(200); tooHigh = SensorValue(LS); //get white light value PlaySound(soundBlip); offset = ((tooLow+tooHigh)/2)+2; //get light sensor average for offset wait10Msec(100); HTMAGstartCal(MS); nMotorPIDSpeedCtrl[motorB] = mtrNoReg; //turn motor B's PID off nMotorPIDSpeedCtrl[motorC] = mtrNoReg; //turn motor C's PID off while(true){ //start main PID loop if(HTMAGreadVal(MS) <= -20 || HTMAGreadVal(MS) >= 20){ //pauseing cycle ClearTimer(T1); //start timing while(HTMAGreadVal(MS) <= -20 || HTMAGreadVal(MS) >= 20){ //main pausing loop if(time100(T1) >= 40){ StopAllTasks(); //Abort if timer gets over 4 seconds } motor[motorB] = 0; //turn motor B off motor[motorC] = 0; //turn motor C off } } error = SensorValue(LS)-offset; //calculate difference between offset and light value integral = integral+error; //calculate integral (remember the past) derivative = error-lastError; //calculate derviative (predict the future) turn = (Kp*error)+(Ki*integral)+(Kd*derivative); //summarize PID stuff powerB = Tp-turn; //calculate power for motor B powerC = Tp+turn; //calculate power for motor C motor[motorB] = -powerB; //set motor B to power B in reverse motor[motorC] = -powerC; //set motor C to power C in reverse lastError = error; //update memory wait1Msec(5); } }