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(); }
//----Logs data - for testing only----// void datalogging() { char tempX, tempY, tempTheta; tempX = (char) poseWorld.maxActivatedCell.x; tempY = (char) poseWorld.maxActivatedCell.y; tempTheta = (char) poseWorld.maxActivatedCell.theta; AddToDatalog(1,tempX); AddToDatalog(2,tempY); AddToDatalog(3,tempTheta); AddToDatalog(4,numActive); AddToDatalog(5,'0'); //just a separator }
//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(); }
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; } //--------------------------------- } }