faceObject(){
int min = 255;
for(int i = 0; i < 46; i++){
turn(2);
sleep(10);
hogCPU();
heading = (gHeading + 0.5); // change to sonar reaading
releaseCPU();
if(heading < min){
min = heading;
}
}
sleep(500);
for(int i = 0; i < 91; i++){
turn(-2);
sleep(10);
hogCPU();
heading = (gHeading + 0.5);
releaseCPU();
if(heading < min){
min = heading;
}
}
}
/**
* Turn left for given degrees, negative means turn right
*/
void turn(int degrees)
{
hogCPU();
int heading= (gHeading+0.5);
float dedt = -gRot;
releaseCPU();
int target_heading = heading+degrees;
//turn left if degrees to turn is positive
int full_power =degrees>0? -TURN_POWER:TURN_POWER;
float pid_output=0;
int err=degrees;
float err_integral=0;
long last_action_time;
long prev_meas_time=nSysTime;
while(true)
{
int power =0;
//calculate power based on err.
if(abs(err)>TOLERANCE||abs(dedt)>5.0)
{
pid_output =err*kp+err_integral*ki;
if(abs(dedt)>2)//sensor noise when rotation below 2 degrees/sec
pid_output += kd*dedt;
last_action_time=nSysTime;
if(pid_output<-1)
pid_output=-1;
if(pid_output>1)
pid_output=1;
}
power=full_power*pid_output+0.5;
writeDebugStreamLine("targ: %d, head: %d, power: %d, dedt: %f, int:%f", target_heading, heading,power,dedt,err_integral);
motor[FrontR] = power;
motor[FrontL] = power;
motor[BackR] = power;
motor[BackL] = power;
if(nSysTime-last_action_time>100)
{
//robot has been stable for 100 ms
//we are done turning
break;
}else
{
//keep measuring just to see if it will be stable
}
sleep(5);
//measuring
hogCPU();
heading = (gHeading+0.5);
dedt=-gRot;
releaseCPU();
err=target_heading-heading;
//TODO: we need timestamp measurement in gyro task
err_integral += err*(nSysTime-prev_meas_time)/1000.0;
prev_meas_time=nSysTime;
}
}
int getInt(const string cmdA,const string cmdB, int min, int max){
int value=min;
bool doloop=true;
hogCPU();
while(doloop){
ClearTimer(T4);
nxtDisplayCenteredBigTextLine(1,"%s", cmdA);
nxtDisplayCenteredBigTextLine(3,"%s", cmdB);
nxtDisplayCenteredBigTextLine(5, "%d",value);
while(time10[T4]<20){};
if(nNxtButtonPressed == 1) value++;
if(nNxtButtonPressed == 2) value--;
if(value>max) value=max;
if(value<min) value=min;
if(nNxtButtonPressed == 3) doloop=false;
}
ClearTimer(T4);
while(time10[T4]<100){
nxtDisplayCenteredBigTextLine(1,"%s" ,cmdA);
nxtDisplayCenteredBigTextLine(3,"%s" ,cmdB);
nxtDisplayCenteredBigTextLine(5, "Got %d",value);
releaseCPU();
}
return value;
}
int setSpeed(float v, float w)
{
// start the motors so that the robot gets
// v m/s linear speed and w RADIAN/s angular speed
float w_r = (L * w + 2 * v)/(2*R);
float w_l = (2*v - R*w_r)/R;
// parameters of power/speed transfer
float mR = 5.5058, mL = 5.5092, nR = 1.4976, nL = 1.8269;
float motorPowerRight, motorPowerLeft;
// sets the power for both motors
motorPowerLeft = mL * w_l + nL;
motorPowerRight = mR * w_r + nR + 1.15;
// checks if calculated power exceeds the motors capacity
if (motorPowerLeft <= 100 && motorPowerRight <= 100) {
hogCPU();
motor[motorA] = motorPowerLeft;
motor[motorC] = motorPowerRight;
releaseCPU();
return 1;
}
else {
// too much power
return 0;
}
}
float Gyro_Heading()
{
hogCPU();
float i = heading;
releaseCPU();
return i;
}
task main(){
int spdCmd;
//---------------------------INIT-----------------------------------//
Pid_Init();
//---------------------------END INIT-------------------------------//
// while(SensorValue(endPgm) == 0){
while(true){
ClearTimer(T1);
hogCPU(); //Prevent other tasks from running when this one is.
//writeDebugStreamLine("Foreground\n"); //Let me know when the foreground runs
count=count+1; // Count the number of times the foreground runs.
//--------------------------FOREGROUND------------------------------------//
spdCmd=50;
int mtrCnt=nMotorEncoder[rtMotor];
int mtrCmd=Pid(spdCmd, mtrCnt, mtrCntOld, integral);
motor[rtMotor]=motor[ltMotor]=mtrCmd;
//--------------------------END FOREGROUND--------------------------------//
timeLeft=FOREGROUND_MS-time1[T1]; // Calculate the time used in the foreground
releaseCPU(); // Let other tasks run now.
wait1Msec(timeLeft);// The time other tasks have to run before foreground takes control.
//------------Debug------------//
if (count==1){
writeDebugStreamLine("SpdCmd[%i],Kp[%i],Ki[%i],Kd[%i]",spdCmd,Kp,Ki,Kd);
writeDebugStreamLine("TimeLeft,Error,MtrCmd,Speed,Integral,Distance");
}
writeDebugStreamLine("%2i,%3i,%3i,%3i,%3i,%5i",timeLeft,error,mtrCmd,speed,integral,mtrCnt);
}// While Loop
}// Foreground Main Task
task errorReset()//May work, or may cause catastrophic failue
{
hogCPU();
wait1Msec(1500);
releaseCPU();
startTask(usercontrol);
}
task Foreground(){
int timeLeft;
while(true){
ClearTimer(T1);
hogCPU();
//--------------------Robot Code---------------------------//
long robotDist = nMotorEncoder[rtWheelMotor] + nMotorEncoder[ltWheelMotor];
int robotDir = (int)(nMotorEncoder[ltWheelMotor] - nMotorEncoder[rtWheelMotor]);
// Calculate the speed and direction commands
// int speedCmd = ForwardDist(path[pathIdx][DIST_IDX], robotDist, path[pathIdx][SPD_IDX]);
// int dirCmd=Direction(path[pathIdx][DIR_IDX], robotDir, path[pathIdx][DIRRL_IDX]);
// DebugInt("spd",speedCmd);
// DebugInt("dir",dirCmd);
#define FWDa 1
#define RT90a 2
int sm=FWD1;
int speedCmd=0;
int dirCmd=0;
switch sm {
case FWDa: //Drive Forward
speedCmd = ForwardDist(10, robotDist, 50);// CmdStopDist,,CmdSpeed
dirCmd=Direction(0, robotDir, 50);// CmdDir,,RateLimitValue
break;
default:
}
// Calculate the motor commands given the speed and direction commands.
// motor[ltWheelMotor]=speedCmd+dirCmd;
// motor[rtWheelMotor]=speedCmd-dirCmd;
//--------------------------Robot Code--------------------------//
// Wait for next itteration
timeLeft=FOREGROUND_MS-time1[T1];
releaseCPU();
wait1Msec(timeLeft);
}// While
}//Foreground
task main()
{
//--------------------INIT Code---------------------------//
ForwardDistReset((tMotor)rtWheelMotor, (tMotor)ltWheelMotor);
DirectionReset();
//--------------------End INIT Code--------------------------//
StartTask(Foreground, 255);
while(true){
//-----------------Print the debug statements out------------------//
DebugPrint();
}// While
}// Main
void Arm_SetSpeed(int Speed)
{
hogCPU();
if(Arm_Initialized)
{
Arm_Speed = Speed;
}
releaseCPU();
}
/*
* Sets speed to motors
*/
int setSpeedBase(float v, float w)
{
// Start the motors so that the robot gets
// v m/s linear speed and w RADIAN/s angular speed
float w_r = (L * w + 2 * v)/(2*R);
float w_l = (2*v - R*w_r)/R;
// Parameters of power/speed transfer
float mR = 5.5058, mL = 5.5092, nR = 1.4976, nL = 1.8269;
//float mR = 5.80117, mL = 5.76965, nR = -0.20796, nL = 0.138482;
float motorPowerRight, motorPowerLeft;
// Sets the power for both motors
if(v == 0 && w == 0){
motorPowerLeft = 0;
motorPowerRight = 0;
} else{
motorPowerLeft = mL * w_l + nL;
motorPowerRight = mR * w_r + nR + 1.15;
}
// Set current speed with semaphore
AcquireMutex(access_speed);
curV = v;
curW = w;
ReleaseMutex(access_speed);
// Checks if calculated power exceeds the motors capacity
if (motorPowerLeft <= 80 && motorPowerRight <= 80) {
hogCPU();
motor[motorA] = motorPowerLeft;
motor[motorC] = motorPowerRight;
releaseCPU();
return 1;
} else {
// Too much power, sets the power to the maximum possible
hogCPU();
motor[motorA] = 80;
motor[motorC] = 80;
releaseCPU();
return 0;
}
}
void Arm_Reinitialize()
{
hogCPU();
Arm_Initialized = false;
Arm_BottomEncoderLimit = 0;
Arm_TopEncoderLimit = 0;
Arm_Encoder = 0;
Arm_Speed = 0;
releaseCPU();
}
void putLiftUpAuto(){
//put up lift to lower height
hogCPU();
motor[M_Lift]=100;
int safetime=1200; //1.2 seconds
ClearTimer(T4);
while(nMotorEncoder[M_Lift]<C_LIFTLOWTARGET-200 && time1[T4]<safetime){};
motor[M_Lift]=0;
if(time1[T4]>=safetime) StopAllTasks();
releaseCPU();
}
//#include "JoystickDriver.c"
task main()
{
//waitForStart();
startTask(goforward, 10 );
startTask(raiseZElevator, 10 );
startTask(blow, 10 );
releaseCPU();
wait1Msec(21000);
}
void scoreRingsAuto(){
hogCPU();
motor[M_Lift]=-10;
int safetime=2500;
ClearTimer(T4);
while( nMotorEncoder[M_Lift]>C_LIFTLOWTARGET-1300 && time1[T4]<safetime){
//swivel??
if(time100[T4]%2 == 0) goRot(10);
if(time100[T4]%2 == 1) goRot(-10);
};
allDriveStop();
motor[M_Lift]=0;
if(time1[T4]>=safetime) StopAllTasks();
releaseCPU();
}
//biggest amount you can turn is +/- 180 degrees
void Turn(int Degrees)
{
hogCPU();
Gyro_Reset();
TotalDrift = 0;
releaseCPU();
while(abs(-Degrees + Heading) > 1)
{
Move((-Degrees + Heading) / 2, (Degrees - Heading) / 2)
}
Move(0, 0);
}
void Straight(int Power, int Miliseconds)
{
hogCPU();
Gyro_Reset();
TotalDrift = 0;
releaseCPU();
Time1[T4] = 0;
while(Time1[T4] < Miliseconds)
{
Move(Power + Heading, Power - Heading);
}
Move(0, 0);
}
void putLiftDownAuto(){
//put lift down
int safetime;
hogCPU();
motor[M_Lift]=-50;
safetime=1000;
ClearTimer(T4);
while( nMotorEncoder[M_Lift]>800 && time1[T4]<safetime){};
if(time1[T4]>=safetime) { motor[M_Lift]=0; StopAllTasks(); }
motor[M_Lift]=-10;
safetime=1000;
ClearTimer(T4);
while( nMotorEncoder[M_Lift]>100 && time1[T4]<safetime){};
if(time1[T4]>=safetime) { motor[M_Lift]=0; StopAllTasks(); }
motor[M_Lift]=0;
releaseCPU();
}
task main ()
{
// resets odometry
AcquireMutex(semaphore_odometry);
set_position(robot_odometry, 0, 0, 0);
ReleaseMutex(semaphore_odometry);
// resets motor encoders
hogCPU();
nMotorEncoder[motorA] = 0;
nMotorEncoder[motorC] = 0;
releaseCPU();
StartTask(updateOdometry);
track_wall();
StopTask(updateOdometry);
}
task main(){
while(true){
ClearTimer(T1);
hogCPU(); //Prevent other tasks from running when this one is.
for(long i=0; i<30000; ++i){} //Add some code to see the time used in the foreground. This takes about 0.5 seconds
writeDebugStreamLine("Foreground\n"); //Let me know when the foreground runs
count=count+1; // Count the number of times the foreground runs.
timeLeft=FOREGROUND_MS-time1[T1]; // Calculate the time used in the foreground
releaseCPU(); // Let other tasks run now.
wait1Msec(timeLeft);// The time other tasks have to run before foreground takes control.
writeDebugStreamLine("Count=[%2i] Time Left = %i\n",count,timeLeft);
}// While Loop
}// Foreground Task
task GyroDeviceDriver()
{
float nAngularVelocity;
float kNoiseEliminate = 0;
long nSumOfSamples = 0;
const int kBiasSamples = 400;
const int kSamplingTime = 3; // NXT analog sensor values are scanned by h/w every 3 msec
float fGyroBias = 0; // Each gyro sensor has a bias that needs to be calculated before data is collected
float fVelocityFactor = -0.00307; // A constant used in integrating the readings from the sensor
SensorType[kGyroSensor] = sensorLightInactive; // Gyro behaves like a analog light sensor.
//
// Calculate the gyro bias
//
nSchedulePriority = kHighPriority - 8; // Boost execution priority so that the task runs deterministically
wait1Msec(1500); // Allows the gyro sensor time to adjust to the conditions
for (int nNumbSamples = 0; nNumbSamples < kBiasSamples; ++nNumbSamples)
{
// Calculates the bias by taking an average of the samples
nSumOfSamples += SensorRaw[kGyroSensor];
wait1Msec(kSamplingTime);
}
fGyroBias = (float) nSumOfSamples / kBiasSamples;
kNoiseEliminate = fGyroBias/10;
//
// Ready to do the angle calculations
//
nGyroState = stateGyroWorking;
while (true)
{
wait1Msec(kSamplingTime);
nAngularVelocity = SensorRaw[kGyroSensor] - fGyroBias;
if ((nAngularVelocity > -kNoiseEliminate) && (nAngularVelocity <= kNoiseEliminate))
continue;
hogCPU(); fGyroAngle += (nAngularVelocity * fVelocityFactor)*360.0/510.0; releaseCPU();
}
return;
}
task main()
{
clearDebugStream();
writeDebugStreamLine("This is PIDTest\n");
memset(&desiredEncVals, 0, sizeof(desiredEncVals));
motorInit(&desiredEncVals);
semaphoreInitialize(PIDconstantSemaphore);
startTask(PID);
long loopStartTimeMs = nPgmTime;
semaphoreLock(PIDconstantSemaphore);
for (pid_kp=0; pid_kp<5; pid_kp++) {
for (pid_ki=0; pid_ki<0; pid_ki+=0.01) {
for (pid_kd=0; pid_kd<0; pid_kd++) {
writeDebugStream("%f, %f, %f, ", pid_kp, pid_ki, pid_kd);
if (bDoesTaskOwnSemaphore(PIDconstantSemaphore)) {
semaphoreUnlock(PIDconstantSemaphore);
}
while(motorGetEncoder((tMotor) MecMotor_FR) < 5000){
hogCPU();
motorUpdateState();
desiredMotorVals.power[MecMotor_FR] = 50;
releaseCPU();
}
semaphoreLock(PIDconstantSemaphore);
motor[MecMotor_FR] = 0; //can do this because we have lock on semaphore
writeDebugStream("Changing constants!\n");
wait1Msec(1000); //wait for motor to spin down
}
}
}
}
//the program below uses feedback from encoders to determine how much the robot turns.
task main()
{
int trayectoria = 1; // chooses trajectory to run
float thetaINIT = 0; // initial theta (different for each trajectory)
if (trayectoria == 1) {
thetaINIT = (PI)/2;
}
else if (trayectoria == 2) {
thetaINIT = 0;
}
// reset odometry values and motor encoders
// resets odometry
AcquireMutex(semaphore_odometry);
set_position(robot_odometry, 0, 0, thetaINIT);
ReleaseMutex(semaphore_odometry);
// resets motor encoders
hogCPU();
nMotorEncoder[motorA] = 0;
nMotorEncoder[motorC] = 0;
releaseCPU();
StartTask(updateOdometry);
// executes the required trajectory
if (trayectoria == 1) {
ejecutarTrayectoria1();
}
else if (trayectoria == 2) {
ejecutarTrayectoria2();
}
StopTask(updateOdometry);
Close(hFileHandle, nIoResult);
}
void initGyro()
{
hogCPU();
nxtDisplayBigTextLine(0,"Gyro");
nxtDisplayString(2, "The gyro");
nxtDisplayString(3,"is calibrating!");
nxtDisplayString(5,"DO NOT MOVE THE");
nxtDisplayString(6, " ROBOT.");
wait1Msec(1000);
gyroData.degsec = 0;
gyroData.deg = 0;
gyroData.offset = HTGYROstartCal(GYRO);
eraseDisplay();
releaseCPU();
StartTask(gyroTask, 10);
}
task main(){
// Initialize variables here //
int myInt=0;
int in=0;
int out=0;
//Debug2File(); //Send the debug information to the file debug.txt
//Debug2NXT(); //Send the debug information to the NXT screen
Debug2Stream(); //Send the debug information to the PC Screen
// End of initialize //
while(true){
clearTimer(T1);
hogCPU(); //Prevent other tasks from running when this one is.
// ------------- Put Unit Test code here -------------------//
// xxxxxxx [] Describe test 1 here. Put an X inside of [] when the test passes.
// xxxxxxx [] Describe test 2 here. Put an X inside of [] when the test passes.
// USAGE NOTES:
// The units for a are encoder clicks
// Set #define NO_UNIT_TEST
if (i<5) in=0;
if (i<10) in=1;
out=MySoftwareModule(in);
DebugInt("In",in);
DebugInt("Out",out);
i+=1; // Increment the frame counter for unit test
// ------------- Unit code test is done here ---------------//
DebugPrint();
timeLeft=FOREGROUND_MS-time1[T1]; // Calculate the time used in the foreground
releaseCPU(); // Let other tasks run now.
wait1Msec(timeLeft);// The time other tasks have to run before foreground takes control.
}// While Loop
}// Main Task
task main(){
while(true){
ClearTimer(T1);
hogCPU(); //Prevent other tasks from running when this one is.
getJoystickSettings(joystick); // update buttons and joysticks
eraseDisplay();
//-------------------------- Start of the State Machine ----------------//
switch (sm_cmd) {
case LIGHT_OFF: //State
// Do this when the light is off
nxtDisplayString(2, "Light Off");
if (FallEdge(joy1Btn(1),btn_z1)){ //Transition
// Do this on a transition
sm_cmd=LIGHT_ON;
}
break;
case LIGHT_ON: //State
nxtDisplayString(2, "Light On");
if (FallEdge(joy1Btn(1),btn_z1)){ //Transition
// Do this on a transition
sm_cmd=LIGHT_OFF;
}
}
// ------------------------- End of the State Machine ------------------//
count=count+1; // Count the number of times the foreground runs.
timeLeft=FOREGROUND_MS-time1[T1]; // Calculate the time used in the foreground
releaseCPU(); // Let other tasks run now.
wait1Msec(timeLeft);// The time other tasks have to run before foreground takes control.
writeDebugStreamLine("Count=[%2i] State[%2i]",count,sm_cmd);
}// While Loop
}// Foreground Task
task PIDControl()
{
while(true)
{
SensorValue[FlyWheel] = 0;//Measure difference
releaseCPU();//Oinkless: Do other stuff
wait1Msec(PI*20);//TimeSample to end all time samples
hogCPU();//Oink: Do nothing else
Error = TargetSpeed[n] - SensorValue[FlyWheel];//How much I'm wrong
datalogAddValue(0, Error);//DATADATADATADATADATADATADATADATADATADATADATADATA
KpError = Kp[n]*Error;//Debuggery & Tuning
Integral[n] += (Error + PrevError)/2;//How wrong I've been
datalogAddValue(1, Integral[n]);//DATADATADATADATADATADATADATADATADATADATADATADATA
KiIntegral = Ki[n]*Integral[n];//Debuggery & Tuning
DeltaE = PrevError - Error;//How much less wrong I am
datalogAddValue(2, DeltaE);//DATADATADATADATADATADATADATADATADATADATADATADATA
KdDeltaE = Kd[n]*DeltaE;//Debuggery & Tuning
PrevError = Error;//What it says on the tin
PIDPower = KpError + KdDeltaE + KiIntegral + stillspeed[n];//PID Equation
BangBang = Error > 0 ? 127 : 0;//On or Off, Pure Binary
PIDBang = abs(Error)>AccError[n] ? BangBang : PIDPower;//Axiom of choice
Flyspeed = PIDBang<0?0:PIDBang*(n==0?0:1);//Limit the flywheel to whole numbers and keep 0 range from doing anything
}
}
task Foreground(){
//servoChangeRate[servo2] = 2;
int timeLeft;
int state=0;
while(true){
ClearTimer(T1);
hogCPU();
//--------------------Robot Code---------------------------//
long armEncoder = nMotorEncoder[blockthrower];
long robotDist = nMotorEncoder[rtWheelMotor] + nMotorEncoder[ltWheelMotor];
long robotDir = nMotorEncoder[ltWheelMotor] - nMotorEncoder[rtWheelMotor];
long distInches = robotDist/IN2CLK;
// Calculate the speed and direction commands
int speedCmd = ForwardDist(path[pathIdx][DIST_IDX], robotDist, path[pathIdx][SPD_IDX]);
int dirCmd=Direction(path[pathIdx][DIR_IDX], robotDir);
int armSpd = FlipperArm(armEncoder, armSetPos);
bool IRval;
//calculate when to increment path
if (abs(speedCmd)<3 && abs(dirCmd)<3) pathIdx++;
// State O Follow Path
if (state==0)
{
if (distInches>48)
{
state=1;
}
}
IRval = Delayatrue(1, SensorValue[IR] == 1 || SensorValue[IR] == 2);
// State 1 Look for IR Beacon
if (state==1)
{
speedCmd=10;
if ( IRval)
{
state=7;
}
else
{
state=2;
}
}
// State 2 Follow Path
if (state==2)
{
if (distInches>55)
{
state=3;
}
}
// State 3 Look for IR Beacon
if (state==3)
{
speedCmd=10;
if ( IRval==true)
{
state=7;
}
else
{
state=4;
}
}
// State 4 Follow Path
if (state==4)
{
if (distInches>74)//36
{
state=5;
}
}
// State 5 Look for IR Beacon
if (state==5)
{
speedCmd=10;
if ( IRval==true)
{
state=7;
}
else
{
state=6;
}
}
// State 6 Follow Path
if (state==6)
{
if (pathIdx == 3)//45
{
state=7;
}
}
if (state==7)// flip arm
{
speedCmd=0;
dirCmd = 0;
armSetPos = 2300;
if (abs(armSetPos - armEncoder) <10)
{
countState++;
if(countState == 3)
state=8;
}
}
if (state==8)
{
speedCmd = 0;
dirCmd = 0;
armSetPos = 0;
if (abs(armSetPos) - abs(armEncoder) < 200)
{
if(pathIdx == 3)
{
state=9;
}
}
}
if (state==9)
{
pathIdx = 3;
}
DebugInt("spd",speedCmd);
DebugInt("dir",robotDir/DEG2CLK);
DebugInt("dist",distInches);
DebugInt("path",pathIdx);
DebugInt("state",state);
DebugInt("irval",SensorValue[IR]);
// Calculate when to move to the next path index
int s=sizeof(path)/sizeof(path[0])-1;
DebugInt("siz",s);
if (pathIdx>s) pathIdx=s;
speedCmd = RateLimit(speedCmd, START_RATE,speedCmdZ1 );
motor[ltWheelMotor]=speedCmd+dirCmd;
motor[rtWheelMotor]=speedCmd-dirCmd;
motor[blockthrower]=armSpd;
//--------------------------Robot Code--------------------------//
// Wait for next itteration
timeLeft=FOREGROUND_MS-time1[T1];
releaseCPU();
wait1Msec(timeLeft);
}// While
}//Foreground
// TASK TO BE LAUNCHED SIMULTANEOUSLY to "main"!!
task updateOdometry(){
float cycle = 0.01; // we want to update odometry every 0.01 s
int step = 20; // we want to write odometry data each 20 steps
float dSl,dSr,dS,dx,dy,dT;
float x, y, th;
string odometryString = "";
strcat(odometryString, "odometry = ["); // concatenate string2 into string1
string sFileName = "odometrylog.txt";
CloseAllHandles(nIoResult);
//
// Deletes the file if it already exists
//
Delete(sFileName, nIoResult);
hFileHandle = 0;
OpenWrite(hFileHandle, nIoResult, sFileName, nFileSize);
WriteText(hFileHandle, nIoResult, odometryString);
while (true){
// show each step on screen and write in the string
float timeAux = nPgmTime;
float timeAux2;
// read tachometers, and estimate how many m. each wheel has moved since last update
// RESET tachometer right after to start including the "moved" degrees turned in next iteration
// locks the cpu to modify the motors power
// CPU LOCKED
hogCPU();
dSl = nMotorEncoder[motorA];
dSr = nMotorEncoder[motorC];
nMotorEncoder[motorA] = 0;
nMotorEncoder[motorC] = 0;
releaseCPU();
// CPU RELEASED
// calculates odometry
dSl = R * degToRad(dSl);
dSr = R * degToRad(dSr);
dS = (dSr + dSl) / 2;
dT = (dSr - dSl) / L;
dx = dS * cos(robot_odometry.th + (dT/2));
dy = dS * sin(robot_odometry.th + (dT/2));
x = robot_odometry.x + dx;
y = robot_odometry.y + dy;
th = normTheta(robot_odometry.th + dT);
// updates odometry
AcquireMutex(semaphore_odometry);
set_position(robot_odometry, x, y, th);
ReleaseMutex(semaphore_odometry);
// Write final string into file
if(step==20){
step = 0;
string temp, temp2;
StringFormat(temp, "%.2f, %.2f,", x, y);
StringFormat(temp2, "%.2f; \n", th);
strcat(temp,temp2);
WriteText(hFileHandle, nIoResult, temp);
}
step++;
// Wait until cycle is completed
timeAux2 = nPgmTime;
if ((timeAux2 - timeAux) < (cycle * 1000)) {
Sleep( (cycle * 1000) - (timeAux2 - timeAux) );
}
}
}
task main(){
//--------------------INIT Code---------------------------//
ForwardDistReset((tMotor)rtMotor, (tMotor)ltMotor);
DirectionReset();
nMotorEncoder[blockthrower] = 0;
speedCmdZ1=0;
pathIdx=0;
delayatruecount=0;
int state=0;
Pid_Init1();
Pid_Init2();
//--------------------End INIT Code--------------------------//
waitForStart(); // Wait for the beginning of autonomous phase.
int iFrameCnt=0;
int timeLeft;
servo[irArm]=243;
while(true){
ClearTimer(T1);
hogCPU();
//--------------------Robot Code---------------------------//
long armEncoder = nMotorEncoder[blockthrower];
long robotDist = nMotorEncoder[rtMotor] + nMotorEncoder[ltMotor];
long robotDir = nMotorEncoder[ltMotor] - nMotorEncoder[rtMotor];
long distInches = robotDist/IN2CLK;
long dirDegrees = robotDir/27;
// Calculate the speed and direction commands
int speedCmd = ForwardDist(path[pathIdx][DIST_IDX], robotDist, path[pathIdx][SPD_IDX]);
int dirCmd = Direction(path[pathIdx][DIR_IDX], robotDir);
int armSpd = FlipperArm(armEncoder, armSetPos);
bool IRval;
//calculate when to increment path
if (abs(path[pathIdx][DIR_IDX] - dirDegrees) < 4 && abs(path[pathIdx][DIST_IDX] - distInches) < 3) pathIdx++;
// Calculate the IR value
IRval = Delayatrue(1, SensorValue[IR] == 5 || SensorValue[IR] == 6);
if (state==0)// State O Follow Path
{
if (distInches>28)
{
state=1;
}
}
if(state==1)// State 1 Swing out irArm
{
servo[irArm]=150;
if (distInches>34)
{
state=2;
}
}
if (state==2)// state 2 look for ir under box 1
{
if ( IRval||SensorValue[IR] == 3||SensorValue[IR] == 2)
{
state=12;
servo[irArm]=243;
}
else
{
state=3;
}
}
if (state==12)//follows path before flipping arm
{
//speedCmd=0;
if(distInches>44)
{
state = 8;
}
}
if (state==3)//state 3 look for box 2 and follow path
{
if (distInches>46)
{
state=4;
}
}
if (state==4)//state 4 look for ir under box 2
{
if ( IRval==true||SensorValue[IR] == 3)
{
state=13;
servo[irArm]=243;
}
else
{
state=15;
}
servo[irArm]=243;
}
if (state==13) // waits for distance before flipping
{
if(distInches>57)
{
state = 8;
}
}
if (state==15) // pulls servo arm out
{
if(distInches>57)
{
servo[irArm] = 80;
state =5;
}
}
if (state==5)//state 5 look for box 3 and follow path
{
if (distInches>66)//36
{
state=6;
}
}
if (state==6)// State 6 Look for ir under box 3
{
if ( IRval||SensorValue[IR] == 4||SensorValue[IR] == 3||SensorValue[IR] == 2)
{
state=14;
}
else
{
state=7;
}
servo[irArm]=243;
}
if (state==14)// waits distance before flipping arm
{
if(distInches>69)
state = 8;
}
if (state==7)// State 7 look for box 4
{
if (pathIdx == 3)//45
{
state=8;
}
}
if (state==8)// state 8 flip arm
{
servo[irArm]=243;
speedCmd=0;
dirCmd = 0;
armSetPos = 2300;
if (abs(armSetPos - armEncoder) <10)
{
state=9;
}
}
if (state==9)//state 9 lower arm
{
speedCmd = 0;
dirCmd = 0;
armSetPos = 0;
if (abs(armSetPos - armEncoder) < 400)
{
pathIdx=3;
state=10;
}
}
if(state==10)//state 10 follow path
{
}
//DebugInt("spd",speedCmd);
//DebugInt("dir",robotDir/DEG2CLK);
//DebugInt("dist",distInches);
//DebugInt("path",pathIdx);
//DebugInt("state",state);
DebugInt("irval",SensorValue[IR]);
// Calculate when to move to the next path index
int s=sizeof(path)/sizeof(path[0])-1;
if (pathIdx>s) pathIdx=s; // Protect the path index
// Ramp the command up
speedCmd = RateLimit(speedCmd, START_RATE,speedCmdZ1);
leftmotors=Pid1(speedCmd+dirCmd);
rightmotors=Pid2(speedCmd-dirCmd);
//rightmotors=speedCmd-dirCmd;
//leftmotors=speedCmd+dirCmd;
motor[rtBack]=rightmotors;
motor[rtMotor]=rightmotors;
motor[ltMotor]=leftmotors;
motor[ltBack]=leftmotors;
motor[blockthrower]=armSpd;
//DebugInt("rightmotors",rightmotors);
//DebugInt("leftmotors",leftmotors);
//DebugInt("rightencoder",nMotorEncoder[rtMotor]);
//DebugInt("leftencoder",nMotorEncoder[ltMotor]);
if (iFrameCnt==0)
writeDebugStreamLine("i,pthIdx,rbtDist,irval,spdCmd,IR,state, rightencoder, leftencoder");
writeDebugStreamLine("%3i,%3i,%4i,%4i,%3i,%3i,%3i, %4i, %4i",iFrameCnt,pathIdx,distInches,IRval,speedCmd,SensorValue[IR],state, nMotorEncoder[rtMotor], nMotorEncoder[ltMotor]);
//--------------------------Robot Code--------------------------//
// Wait for next itteration
iFrameCnt++;
timeLeft=FOREGROUND_MS-time1[T1];
releaseCPU();
wait1Msec(timeLeft);
}// While
}//Foreground
task Foreground() {
//servoChangeRate[servo2] = 2;
Pid_Init1();
Pid_Init2();
int timeLeft;
int state=0;
int speedCmd = 0;
int dirCmd = 0;
servo[irArm]=105;
const tMUXSensor LEGOUS = msensor_S4_3;
while(true) {
ClearTimer(T1);
hogCPU();
//--------------------Robot Code---------------------------//
long armEncoder = nMotorEncoder[blockthrower];
long robotDist = nMotorEncoder[rtMotor] + nMotorEncoder[ltMotor];
long robotDir = nMotorEncoder[ltMotor] - nMotorEncoder[rtMotor];
long distInches = robotDist/IN2CLK;
long dirDegrees = robotDir/DEG2CLK;
// Calculate the speed and direction commands
if(shortPathTrue == false)
{
speedCmd = ForwardDist(path[pathIdx][DIST_IDX], robotDist, path[pathIdx][SPD_IDX]);
dirCmd=Direction(path[pathIdx][DIR_IDX], robotDir);
}
else
{
speedCmd = ForwardDist(shortPath[pathIdx][DIST_IDX], robotDist, shortPath[pathIdx][SPD_IDX]);
dirCmd=Direction(shortPath[pathIdx][DIR_IDX], robotDir);
}
int armSpd = FlipperArm(armEncoder, armSetPos);
bool IRval;
bool SonarVal;
//calculate when to increment path
if (abs(path[pathIdx][DIR_IDX] - dirDegrees) < 4 && abs(path[pathIdx][DIST_IDX] - distInches) < 3) pathIdx++;
// State O Follow Path
if (state==0)
{
if (distInches>18)
{
state=1;
}
}
IRval = Delayatrue(1, SensorValue[IR] == 4 || SensorValue[IR] == 3);
// State 1 Look for IR Beacon
if (state==1)
{
speedCmd=10;
if ( IRval)
{
state=7;
}
else
{
state=2;
}
}
// State 2 Follow Path
if (state==2)
{
if (distInches>27)
{
state=3;
}
}
// State 3 Look for IR Beacon
if (state==3)
{
speedCmd=10;
if ( IRval==true)
{
state=7;
}
else
{
state=4;
}
}
// State 4 Follow Path
if (state==4)
{
if (distInches>46)//36
{
state=5;
}
}
// State 5 Look for IR Beacon
if (state==5)
{
speedCmd=10;
if ( IRval==true)
{
state=7;
}
else
{
state=6;
}
}
// State 6 Follow Path
if (state==6)
{
if (pathIdx == 1)//45
{
state=7;
}
}
if (state==7)// flip arm
{
speedCmd=0;
dirCmd = 0;
armSetPos = 1900;
if (abs(armSetPos - armEncoder) <20)
{
state=8;
}
servo[irArm]=243;
}
if (state==8)
{
speedCmd = 0;
dirCmd = 0;
armSetPos = 0;
if (abs(armSetPos) - abs(armEncoder) < 200)
{
state=9;
}
}
SonarVal = Delayatrue2(1, USreadDist(LEGOUS) > 40);
if (state==9)
{
if ((distInches>90) && (distInches<115))
{
if(SonarVal)
{
state=10;
}
else
{
state=11;
}
}
}
if(state==10)
{
shortPathTrue=true;
}
if(state==11)
{
}
/*
DebugInt("spd",speedCmd);
DebugInt("dir",robotDir/DEG2CLK);
DebugInt("sonarval",SonarVal);
DebugInt("path",pathIdx);
DebugInt("state",state);
DebugInt("dist",distInches);
DebugInt("ir", SensorValue[IR]);
*/
writeDebugStreamLine("i,pthIdx,rbtDist,irval,spdCmd,IR,state, rightencoder, leftencoder");
writeDebugStreamLine("%3i,%3i,%4i,%4i,%3i,%3i,%3i, %4i, %4i",iFrameCnt,pathIdx,distInches,IRval,speedCmd,SensorValue[IR],state, nMotorEncoder[rtMotor], nMotorEncoder[ltMotor]);
// Calculate when to move to the next path index
int s=sizeof(path)/sizeof(path[0])-1;
DebugInt("siz",s);
if (pathIdx>s) pathIdx=s;
speedCmd = RateLimit(speedCmd, START_RATE,speedCmdZ1 );
rightmotors=speedCmd-dirCmd;
leftmotors=speedCmd+dirCmd;
motor[rtBack]=rightmotors;
motor[rtMotor]=rightmotors;
motor[ltMotor]=leftmotors;
motor[ltBack]=leftmotors;
motor[blockthrower]=armSpd;
DebugInt("rightmotors",rightmotors);
DebugInt("leftmotors",leftmotors);
//--------------------------Robot Code--------------------------//
// Wait for next itteration
timeLeft=FOREGROUND_MS-time1[T1];
releaseCPU();
wait1Msec(timeLeft);
}// While
}//Foreground
