task main ()
{
nxtDisplayCenteredTextLine(3, "Roaming");
nxtDisplayCenteredTextLine(5, "This is a test");
while(nextEmptyCell<numLocalCells)
{
alive();
char lastCellNum = nextEmptyCell;
nSyncedMotors = synchBC;
nSyncedTurnRatio = 101;
nMotorEncoderTarget[motorB] = 200;
motor[motorB] =60;
while(nMotorRunState[motorB] != runStateIdle) {}
setTemp();
checkLocalCell();
doTurn();
if(lastCellNum != nextEmptyCell)
{
AddToDatalog(1);
}
else
{
AddToDatalog(0);
}
// wait1Msec(200);
}
SaveNxtDatalog();
StarWars();
}
//This code measures how quickly a battery will drain (please start as fully charged)//
task main()
{
waitForStart();
nMotorEncoder(Right) = 0;
nMotorEncoder(Left) = 0;
wait1Msec(50);
ClearTimer(T2);
motor[Right] = speed;
motor[Left] = -speed;
AddToDatalog(speedDat, speed);
wait1Msec(8000);
//Get up to speed//
ClearTimer(T1);
startVolts = nAvgBatteryLevel;
while(time1(T1) < 10000){
}
//Starting encoder ticks per second//
encSpeed = ((nMotorEncoder(Right) / 10) + (abs(nMotorEncoder(Left)) / 10)) / 2;
AddToDatalog(encSpeedDat, encSpeed);
AddToDatalog(startVoltsDat, startVolts);
newSpeed = encSpeed;
//Run while at 80% of original speed or greater//
while(newSpeed >= (encSpeed * 80 / 100)){
ClearTimer(T1);
nMotorEncoder(Right) = 0;
nMotorEncoder(Left) = 0;
wait1Msec(50);
while(time1(T1) < 10000){
}
//Current encoder ticks per second//
newSpeed = ((nMotorEncoder(Right) / 10) + (abs(nMotorEncoder(Left)) / 10)) / 2;
AddToDatalog(newSpeedDat, newSpeed);
AddToDatalog(newVoltsDat, nAvgBatteryLevel);
}
time = time100(T2) / 10;
AddToDatalog(timeDat, time);
SaveNxtDatalog();
}
//----main----//
task main ()
{
Delete(sFileName1, nIoResult);
Delete(sFileName2, nIoResult);
nxtDisplayCenteredTextLine(3, "Roaming");
nxtDisplayCenteredTextLine(5, "This is a test");
initialisePose(); //set up
iterate(stepSize); //run excitation etc
currentDirection = 0; //set initial
currentTheta = 0;
setTemp(); //get local view
checkLocalCell(); //create first association
datalogging();
sumPoseStruct();
while(1)
{
alive(); //stop NXT from sleeping
float centreSonarValue = SensorValue(centreSonar);
if(centreSonarValue<19)
{
doTurn();
pose3D(changeTheta,0);
}
else {
drive(100,180,50);
pose3D(changeTheta,0.5);
}
sumPoseStruct();
setTemp();
checkLocalCell();
//store data
datalogging();
clearEncoders(); //clear encoder count
changeTheta=0;
}
SaveNxtDatalog();
PlaySound(soundException);
while(bSoundActive){}
}
//----main----//
task main ()
{
nxtDisplayCenteredTextLine(3, "Pose Test");
wait1Msec(500);
initialisePose(); //set up
iterate(stepSize); //run excitation etc
currentDirection = 0; //set initial
currentTheta = 0;
changeTheta = 0;
//display data
displayMax();
nxtDisplayTextLine(2, "Num Act.: %3d",numActive);
nxtDisplayTextLine(4, "Direction: %1d", currentDirection);
nxtDisplayTextLine(5, "currentTheta:%3d", currentTheta);
nxtDisplayTextLine(6, "changeTheta:%3d", changeTheta);
datalogging();
while(totalClicks<1800)
{
alive(); //stop NXT from sleeping
totalClicks += clicks;
//drive(-100,190,50);
// drive(50,180,50);
pose3D(changeTheta,0.5);
displayMax();
nxtDisplayTextLine(2, "Num Act.: %3d",numActive);
nxtDisplayTextLine(4, "Direction: %1d", currentDirection);
nxtDisplayTextLine(5, "currentTheta:%3d", currentTheta);
nxtDisplayTextLine(6, "changeTheta:%3d", changeTheta);
clearEncoders(); //clear encoder count
changeTheta=0;
datalogging();
}
PlaySound(soundFastUpwardTones);
while(bSoundActive) {}
SaveNxtDatalog();
}
task main()
{
nMotorEncoder[motorB] = 0; // Reset the Motor Encoder of Motor B
/* Note: nMotorEncoder has a range of -32768 to 32767. So it will "wrap" after about ~90
revolutions. Hence, nMotorEncoder must be reset often enough to avoid any unexpected
results */
nMotorPIDSpeedCtrl[motorB] = mtrNoReg; // disable PID speed control
nSyncedMotors = synchNone; // ensure no syncronization between motors (i.e.) motorB is independent
float Kp = 0.454*10.5, Ki = 2.2*0.2, Kd = 0.159*0.2; // PID variables. For Lego DC Motor: Ku = 11, Tu = 0.2
/* Note: PID Tuning methods
1. Ziegler-Nichols (Z-N) method:
a. Set Ki and Kd to zero.
b. Slowly increase Kp to a value Ku at which sustained oscillations - constant amplitude and periodic - are observed.
c. Note period of oscillation
d. Refer table below for initial values
+----------------+----------+----------+----------+
| Z-N Model | Kp | Ki | Kd |
+----------------+----------+----------+----------+
| P controller | 0.5*Ku | 0 | 0 |
+----------------+----------+----------+----------+
| PI controller | 0.455*Ku | 0.833*Tu | 0 |
+----------------+----------+----------+----------+
| PID controller | 0.588*Ku | 0.5*Tu | 0.125*Tu |
+----------------+----------+----------+----------+
2. Tyreus-Luyben (T-L) method:
Since Z-N method usually results in aggressive tuning, alternatively T-L method can be adopted (Refer table below).
+----------------+----------+--------+----------+
| T-L Model | Kp | Ki | Kd |
+----------------+----------+--------+----------+
| PI controller | 0.312*Ku | 2.2*Tu | 0 |
+----------------+----------+--------+----------+
| PID controller | 0.454*Ku | 2.2*Tu | 0.159*Tu |
+----------------+----------+--------+----------+
*/
float new_err = 0, old_err = 0, sum_err = 0; // Error variables
int dt = 20; // in ms
int motorPIDpower, motorPIDdegrees;
motor[motorB] = 50; // Motor B given a power level of 50/100
time1[T1] = 0; // arrays to hold the current value of the time in 1ms duration
nDatalogSize = RECORD_SIZE*RUN_TIME/dt; // Allocate memory for datalog
while (time1[T1] < RUN_TIME) { // Run for RUN_TIME sec
motorPIDdegrees = nMotorEncoder[motorB]; // degrees rotated by motor
new_err = ENCODER_SETPOINT-motorPIDdegrees; // Calculate current error
sum_err += new_err*dt/1000; // Integral of error ( calculated as a discrete sum of errors)
// Calculate power to be applied to the motor
motorPIDpower = Kp*new_err + Ki*sum_err + Kd*(new_err-old_err)*1000/dt;
old_err = new_err;
// Ensure power level is between -100 and 100
if(motorPIDpower >= 100)
motorPIDpower = 100;
else if(motorPIDpower <= -100)
motorPIDpower = -100;
motor[motorB] = motorPIDpower; // Apply the calculated power to the motors
AddToDatalog(1,motorPIDdegrees); // store value
wait1Msec(dt); // log data every dt ms
}
motor[motorB] = 0; // Stop the motors
SaveNxtDatalog(); // save log file from RAM to Flash memory
}
task main(){
waitForStart();
int state = -1;
ClearTimer(T1);
while(true){
getJoystickSettings(joystick);
//----------Driving----------------
//Turn Right
if(joystick.joy1_TopHat==2){
motor[frontLeftWheel]=100;
motor[frontRightWheel]=0;
motor[backLeftWheel]=100;
motor[backRightWheel]=0;
if(state != 2){
AddToDatalog(2);
AddToDatalog(time1(T1));
state = 2;
ClearTimer(T1);
nxtDisplayTextLine(3, "right");
}
}
//Turn Left
else if(joystick.joy1_TopHat==6){
motor[frontLeftWheel]=0;
motor[frontRightWheel]=100;
motor[backLeftWheel]=0;
motor[backRightWheel]=100;
if(state != 6){
AddToDatalog(6);
AddToDatalog(time1(T1));
state = 6;
ClearTimer(T1);
nxtDisplayTextLine(3, "left");
}
}
//Move Forward
else if(joystick.joy1_TopHat==0){
motor[frontLeftWheel]=100;
motor[frontRightWheel]=100;
motor[backLeftWheel]=100;
motor[backRightWheel]=100;
if(state != 0){
AddToDatalog(0);
AddToDatalog(time1(T1));
state = 0;
ClearTimer(T1);
nxtDisplayTextLine(3, "up");
}
}
//Move Backward
else if(joystick.joy1_TopHat==4){
motor[frontLeftWheel]=-100;
motor[frontRightWheel]=-100;
motor[backLeftWheel]=-100;
motor[backRightWheel]=-100;
if(state != 4){
AddToDatalog(4);
AddToDatalog(time1(T1));
state = 4;
ClearTimer(T1);
nxtDisplayTextLine(3, "down");
}
}
//Stop
else{
motor[frontLeftWheel]=0;
motor[frontRightWheel]=0;
motor[backLeftWheel]=0;
motor[backRightWheel]=0;
if(state != -1){
AddToDatalog(-1);
AddToDatalog(time1(T1));
state = -1;
ClearTimer(T1);
nxtDisplayTextLine(3, "stop");
}
}
//Save
if(joy1Btn(1) == 1){
SaveNxtDatalog();
nxtDisplayTextLine(3, "save");
return;
}
//---------------------------------
}
}
