task main() { int y=0; // output of rotation sensor int uc=0; // actuator command signal int uc_mag = 0; // magnitude of actuator command int power_level = 0; // power level to motor int time=0; // time int e=0; // error signal int r = 0; // reference signal int counter = 0; //counter on number of loops for program SetSensor(SENSOR_1,SENSOR_ROTATION); // initialize sensors ClearTimer(0); // init timer 0 to 0 CreateDatalog(200); // datalog for 100 points r = 32; while (counter <= 100) { y = SensorValue(0); //read sensor 1 time = Timer(0); AddToDatalog(time); AddToDatalog(y); // compute control e = r - y; uc = e/4 ; // actuator command /* note: gain is 1/3, but we have to deal with integer values and division so it is implemented this way. The resulting uc is an integer value.*/ /* The next commands convert the actuator command into an appropriate power level and sets the direction of the motor. These commands assume that uc is mostly in the range of 0 to 9. If not, then lower the gain. */ uc_mag = abs(uc); // to get the magnitude of the actuator command if (uc_mag == 0) { // set motor to float Float(OUT_A); } else { if (uc_mag > 8) { uc_mag = 8; // must saturate the command } power_level = uc_mag - 1; // power level range is 0 to 7. /* Note: power_level of 0 is not zero power! It is the lowest nonzero level. That is why we use the float for zero power.*/ SetPower(OUT_A,power_level); // set motor power level switch(sign(uc)) { // set direction of motor case 1: Rev(OUT_A); On(OUT_A); break; case 0: Float(OUT_A); // "off" but not "braked" break; case -1: Fwd(OUT_A); On(OUT_A); break; } } counter++; } Off(OUT_A); }