task main() { int sum; float ax,ay,az; int dt,st,en; dt=10; float sumx,sumy,sumz; ClearTimer(T1); time1(T1)=st; HTACreadAllAxes(accel,ax,ay,az); sumx+=ax*(dt/1000)*(dt/1000); sumy+=ay*(dt/1000)*(dt/1000); sumz+=az*(dt/1000)*(dt/1000); nxtDisplayTextLine(1,"%f",sumx); nxtDisplayTextLine(2,"%f",sumy); nxtDisplayTextLine(3,"%f",sumz); time1(T1)=en; dt=en-st; while(true) { HTACreadAllAxes(accel,ax,ay,az); sumx+=ax*(dt/1000)*(dt/1000); sumy+=ay*(dt/1000)*(dt/1000); sumz+=az*(dt/1000)*(dt/1000); nxtDisplayTextLine(1,"%f",sumx); nxtDisplayTextLine(2,"%f",sumy); nxtDisplayTextLine(3,"%f",sumz); } }
// See diagrams: angle = arctan(z/x), // where gyro sensor faces up, with white part // is furthest from locus of movement float angleFromAccel(tSensors sensor) { int x, y, z; float angle; // in radians HTACreadAllAxes(sensor, x, y, z); angle = atan((float) z / (float) x); return radiansToDegrees(angle); }
//Initialization: don't touch void initializeRobot() { HTGYROstartCal(gyro); //Gyro calibration distanceCalibration(); //Calculating correction constants initializeAssignments(); //Filling assignment array HTACreadAllAxes(HTAC, xcal, ycal, zcal); //Calibrating accelerometer return; }
//************************ //Configura sensores e joga erros na tela //************************ void F_STATE_CONFIG() { MTASK_SET_RUN(MT_BEEP); //Enquanto o estado atual for o estado target eraseDisplay(); nxtDisplayCenteredBigTextLine(3, "WAIT"); while(ESTADO_IS_CURRENT()) { //===================================== wait10Msec(100); //Seta os tipos de porta no NXT SensorType[PORT_ARD] = sensorI2CCustom; SensorType[PORT_ACC] = sensorI2CCustom; SensorType[PORT_COMP] = sensorI2CCustom; //Inicia Arduino while(!ERRO_SET_CODE(" CF: ARDU ERR",ARDUinit(PORT_ARD))); //Inicia Acc Sensor while(!ERRO_SET_CODE(" CF: ACC ERR",HTACinit(PORT_ACC))); HTACreadAllAxes(PORT_ACC,ACCEL[0],ACCEL[1],ACCEL[2]); ACCEL_Offset=ACCEL[ACCEL_AXIS_RAMPA]; //Inicia Line Leader while(!ERRO_SET_CODE(" CF: LL ERR",LLinit(PORT_LL))); //Inicia Bussola while(!ERRO_SET_CODE(" CF: COMP ERR",HTMCreadHeading(PORT_COMP)>=0)); //Testa motores while(!ERRO_SET_CODE(" CF: MOTOR A",TEST_MOTOR(MA))); while(!ERRO_SET_CODE(" CF: MOTOR C",TEST_MOTOR(MC))); bMotorReflected[MA] = true; bMotorReflected[MC] = true; GARRA_H(PORT_ARD, GARRA_H_FECHA); wait10Msec(20); GARRA_V(PORT_ARD,GARRA_V_SOBE); wait10Msec(30); GARRA_V(PORT_ARD,GARRA_V_PARA); GARRA_H(PORT_ARD, GARRA_H_PARA); setSafe(3,300); setSafe(4,300); ESTADO_SET_TARGET(ST_WAIT); //===================================== } }
task main() { int _x_axis1 = 0; int _y_axis1 = 0; int _z_axis1 = 0; int _x_axis2 = 0; int _y_axis2 = 0; int _z_axis2 = 0; string _tmp; nxtDisplayCenteredTextLine(0, "HiTechnic"); nxtDisplayCenteredBigTextLine(1, "Accel"); nxtDisplayCenteredTextLine(3, "Test 1"); nxtDisplayCenteredTextLine(5, "Connect sensor"); nxtDisplayCenteredTextLine(6, "to S1"); wait1Msec(2000); // PlaySound(soundBeepBeep); // while(bSoundActive); while (true) { eraseDisplay(); // You can read the three axes one by one // HTACreadX(HTAC, _x_axis1); // HTACreadY(HTAC, _y_axis1); // HTACreadZ(HTAC, _z_axis1); // It's better to read them all at once, if you want to know // all of them anyway. It is a lot more efficient. if (!HTACreadAllAxes(HTAC, _x_axis2, _y_axis2, _z_axis2)) { nxtDisplayTextLine(4, "ERROR!!"); wait1Msec(2000); StopAllTasks(); } nxtDisplayTextLine(0,"HTAC Test 1"); // We can't provide more than 2 parameters to nxtDisplayTextLine(), // so we'll do in two steps using StringFormat() nxtDisplayTextLine(2, "T X Y Z"); StringFormat(_tmp, "S:%4d %4d", _x_axis1, _y_axis1); nxtDisplayTextLine(3, "%s %4d", _tmp, _z_axis1); StringFormat(_tmp, "A:%4d %4d", _x_axis2, _y_axis2); nxtDisplayTextLine(4, "%s %4d", _tmp, _z_axis2); nxtDisplayTextLine(6, "S: 1 by 1"); nxtDisplayTextLine(7, "A: All at once"); wait1Msec(100); } }
sub MTASK_DOTASK(int MTASK_ID){ switch (MTASK_ID) { //******** case MT_DEFAULT: wait1Msec(1); break; //******** case MT_BEEP: PlayTone(200, 12); wait10Msec(120); break; //******** case MT_STOP_BUTTON: if(nNxtButtonPressed==BT_ENTER) { int static TimeDif; TimeDif=time10[T4]; while(nNxtButtonPressed==BT_ENTER){ if(time10[T4]-TimeDif>50) { MV_StopMotors(); ClearSounds(); PlaySound(soundBlip); ESTADO_SET_TARGET(ST_WAIT); break; } } } break; //******** case MT_ACCEL: static int LastTime; if(time100[T4]-LastTime >= 3) { HTACreadAllAxes(PORT_ACC,ACCEL[0],ACCEL[1],ACCEL[2]); if(abs(ACCEL[ACCEL_AXIS_RAMPA]-ACCEL_Offset)>ACCEL_DELTA){ if (ACCEL_Filter++ > 2) ACCEL_Rampa = true; }else{ ACCEL_Filter = 0; ACCEL_Rampa = false; } LastTime=time100[T4]; } break; //******** case MT_TOQUE: wait1Msec(1); break; } }
task getaccel() { while(true) { ClearTimer(T1); HTACreadAllAxes(Accel, X_axis, Y_axis, Z_axis); X_axis = X_axis - offset_X; wait10Msec(1); X_axis_old = X_axis; } }
task main() { //Set up precision speed control (which, incidentally, uses PID to do it :D) nMotorPIDSpeedCtrl[left] = mtrSpeedReg; nMotorPIDSpeedCtrl[right] = mtrSpeedReg; //Initialize PIDs. PID leftPID,rightPID,accelPID; initPID(leftPID,3.5,0,0); initPID(rightPID,3.5,0,0); initPID(accelPID,2,0,0); INTR velIntr; initIntr(velIntr); //Tuning tips: http://robotics.stackexchange.com/questions/167/what-are-good-strategies-for-tuning-pid-loops //Also, double PID loops: http://forum.arduino.cc/index.php?topic=197688.0 //PID for physical position, not just rotational? (need accel) //Swag: https://www.youtube.com/watch?v=n_6p-1J551Y //Maybe we should try a unisphere balancing robot :) https://www.youtube.com/watch?v=bI06lujiD7E //Stands up and initiates gyro stuff. initSweg(); //waitForStart(); //Initialize steering, raise arms... let's start!! StartTask(steer); servo[rearFlipper] = REAR_LIFT_RAISED; servo[frontFlipper] = FRONT_LIFT_RAISED; EndTimeSlice(); while(true) { int ax,ay,az; HTACreadAllAxes(accel, ax, ay, az); float yvel = integrate(velIntr,ay); if(abs(forwardsAngle) > 50) {//If it fell over, reset and redo. reset(leftPID); reset(rightPID); initSweg(); servo[rearFlipper] = REAR_LIFT_RAISED; servo[frontFlipper] = FRONT_LIFT_RAISED; } //Separate left and right PIDs to allow steering by NeutralAngle adjustment. motor[left] = updatePID(leftPID, leftNeutral - forwardsAngle); motor[right] = updatePID(rightPID, rightNeutral - forwardsAngle); motor[left] = -motor[right]; wait1Msec(5); } }
task Display() { while(true) { HTACreadAllAxes(excel,excel_x_raw,excel_y_raw,excel_z_raw); excel_x = ((excel_x_raw/100.0)/GRAVY)+2.0;//cm/s^2 excel_y = (excel_y_raw/100.0)/GRAVY; excel_z = (excel_z_raw/100.0)/GRAVY; eraseDisplay(); nxtDisplayString(3,"x: %d", excel_x_raw); nxtDisplayString(4,"y: %d", excel_y_raw); nxtDisplayString(5,"z: %d", excel_z_raw); } }
task main() { initializeRobot(); StartTask(Drive); const float Conversion = 9.8/200; int X_axis; int Y_axis; int Z_axis; int time; int delta_time; int time_old; int accel_old; int accel_new; float velocity_old; float velocity_new; float distance; clearDebugStream(); ClearTimer(T3); time_old = nPgmTime; accel_old = 0.0; velocity_old = 0; distance = 0; while(true) { HTACreadAllAxes(Accel, X_axis, Y_axis, Z_axis); time = nPgmTime; // this timer wraps around delta_time = abs(time - time_old); // delta time in ms if (delta_time < 1) // protect against divide by zero { delta_time = 1; } accel_new = X_axis; velocity_new = velocity_old + 0.001 * (float)delta_time * 0.5 *(accel_new + accel_old); distance = distance + 0.001 * (float)delta_time * 0.5 *(velocity_new + velocity_old); time_old = time; accel_old = accel_new; velocity_old = velocity_new; writeDebugStreamLine("X:%d, Y:%d, Z:%d\nVelocity:%f, Distance:%f",X_axis, Y_axis, Z_axis, velocity_new,distance); wait10Msec(3); } }
task main() { int x; int y; int z; while(true) { HTACreadAllAxes(excel,x,y,z); eraseDisplay(); nxtDisplayString(3,"x: %d", x); nxtDisplayString(4,"y: %d", y); nxtDisplayString(5,"z: %d", z); } }
task main() { initializeRobot(); const float Conversion = 9.8/200; float time = 0; int X_axis_old =0; int Velocity = 0; int Velocity_old = 0; float Distance = 0,distation = 0; //int Distance_old = 0; int offsetagv_X = 0 ,offsetagv_Y = 0; float theta_X, phi_Y; clearDebugStream(); for(int i = 0; i <= 20; i++) { HTACreadAllAxes(Accel, X_axis, Y_axis, Z_axis); offsetagv_X += abs(X_axis); offsetagv_Y += abs(Y_axis); offset_Z += abs(Z_axis); wait1Msec(100); } StartTask(getaccel); offset_X = (float) (offsetagv_X/20); offset_Y = (float) (offsetagv_Y/20); offset_Z = (float) (offset_Z/20); theta_X = atan((offset_X)/(sqrt((offset_Z*offset_Z)+(offset_Y*offset_Y)))); phi_Y = atan((offset_Y)/(sqrt((offset_Z*offset_Z)+(offset_X*offset_X)))); while(Distance <= distation) { time = time1[T1]/1000; Velocity = (time)*(X_axis+X_axis_old)/2+Velocity_old; Distance = time*(Velocity+Velocity_old)/2+Distance; Velocity_old += Velocity; wait10Msec(1); } }
task main () { int _x_axis = 0; int _y_axis = 0; int _z_axis = 0; string _tmp; nxtDisplayCenteredTextLine(0, "HiTechnic"); nxtDisplayCenteredBigTextLine(1, "Accel"); nxtDisplayCenteredTextLine(3, "Test 1"); nxtDisplayCenteredTextLine(5, "Connect sensor"); nxtDisplayCenteredTextLine(6, "to S1"); wait1Msec(2000); PlaySound(soundBeepBeep); while(bSoundActive) EndTimeSlice(); while (true) { eraseDisplay(); // Read all of the axes at once if (!HTACreadAllAxes(HTAC, _x_axis, _y_axis, _z_axis)) { nxtDisplayTextLine(4, "ERROR!!"); wait1Msec(2000); StopAllTasks(); } nxtDisplayTextLine(0,"HTAC Test 1"); // We can't provide more than 2 parameters to nxtDisplayTextLine(), // so we'll do in two steps using StringFormat() nxtDisplayTextLine(2, " X Y Z"); StringFormat(_tmp, "%4d %4d", _x_axis, _y_axis); nxtDisplayTextLine(3, "%s %4d", _tmp, _z_axis); wait1Msec(100); } }
//Moving function: enter coordinates and the robot does the rest. Keeps track of position by time-based dead reckoning. //Calibration while running a longer program is possible by assigning negative x- and y-coordinates (the robot will push itself into a corner of the field), //and then running calibrate (6) void slitherto(float xgoal, float ygoal, float rgoal) { float alfa; float beta; float _x1 = 0; float _y1 = 0; float _x1s = 0; float _y1s = 0; float dirx; float diry; bool atgoal = false; float distancecm; float betacor; float spd = 30; float _x2 = 0; float rotspeed; atgoal = !((dirx == 1 && xgoal > xcur) || (dirx == -1 && xgoal < xcur) || (diry == 1 && ygoal > ycur) || (diry == -1 && ygoal < ycur) || round(rcur) != rgoal); beta = atan2(ygoal-ycur,xgoal-xcur); betacor = beta + degreesToRadians(rcur); _x1 = cos(betacor); _y1 = sin(betacor); dirx = sgn(xgoal-floor(xcur)); diry = sgn(ygoal-floor(ycur)); time1[T1] = 0; while(atgoal == false) { { if ((dirx == 1 && xgoal > xcur) || (dirx == -1 && xgoal < xcur)) { _x1s = 1; } else { _x1s = 0; } if ((diry == 1 && ygoal > ycur) || (diry == -1 && ygoal < ycur)) { _y1s = 1; } else { _y1s = 0; } if (round(rcur) < rgoal - 5 && (_x1s || _y1s)) _x2 = 1; else if (round(rcur) > rgoal + 5 && (_x1s || _y1s)) _x2 = -1; else if (round(rcur) < rgoal && (_x1s || _y1s)) _x2 = 0.3; else if (round(rcur) > rgoal && (_x1s || _y1s)) _x2 = -0.3; else if (round(rcur) < rgoal && !(_x1s || _y1s)) _x2 = 1; else if (round(rcur) > rgoal && !(_x1s || _y1s)) _x2 = -1; if (beta/1.571 - floor(beta/1.571) > 0.785) //Oh radians.... alfa = 1.571 - (beta/1.571 - floor(beta/1.571)); else alfa = beta/1.571 - floor(beta/1.571); wait1Msec(10); distancecm = abs(((1-C_D)/(-0.785*alfa) + 1)*((d0_1s_1+d0_1s_2)/2)*C_T*time1[T1]*0.001); xcur += distancecm*cos(beta); ycur += distancecm*sin(beta); HTACreadAllAxes(HTAC, xacc, yacc, zacc); xacc -= xcal; yacc -= ycal; zacc -= zcal; writeDebugStreamLine("%d,%d",xacc,yacc); rotspeed = HTGYROreadRot(gyro); //Read the current rotation speed rcur += rotspeed * time1[T1]*0.001; //Magic nxtDisplayCenteredBigTextLine(3, "%2.0f", rcur); //Display our current heading on the screen if (abs(xacc) > 120 || abs(yacc) > 120) { PlaySound(soundBeepBeep); //Move in opposite direction motor[fr] = spd*(_x1*_x1s-_y1*_y1s); motor[br] = spd*(-_x1*_x1s-_y1*_y1s); motor[bl] = spd*(-_x1*_x1s+_y1*_y1s); motor[fl] = spd*(_x1*_x1s+_y1*_y1s); time1[T1] = 0; while (time1[T1] < 1000) { time1[T2] = 0; wait1Msec(10); rotspeed = HTGYROreadRot(gyro); //Read the current rotation speed rcur += rotspeed * time1[T2]*0.001; //Magic nxtDisplayCenteredBigTextLine(3, "%2.0f", rcur); } motor[fr] = 0; motor[br] = 0; motor[bl] = 0; motor[fl] = 0; distancecm = abs(((1-C_D)/(-0.785*alfa) + 1)*((d0_1s_1+d0_1s_2)/2)*C_T*time1[T1]*0.001); xcur += distancecm*cos(-beta); ycur += distancecm*sin(-beta); wait1Msec(1000); } motor[fr] = spd*(-_x1*_x1s+_y1*_y1s-_x2); motor[br] = spd*(_x1*_x1s+_y1*_y1s-_x2); motor[bl] = spd*(_x1*_x1s-_y1*_y1s-_x2); motor[fl] = spd*(-_x1*_x1s-_y1*_y1s-_x2); atgoal = !((dirx == 1 && xgoal > xcur) || (dirx == -1 && xgoal < xcur) || (diry == 1 && ygoal > ycur) || (diry == -1 && ygoal < ycur) || round(rcur) != rgoal); time1[T1] = 0; //Reset timer } motor[fr] = 0; motor[br] = 0; motor[bl] = 0; motor[fl] = 0; } }
sub MTASK_DOTASK(int MTASK_ID){ switch (MTASK_ID) { //******** case MT_DEFAULT: wait1Msec(1); break; //******** case MT_BEEP: PlayTone(200, 12); wait10Msec(120); break; //******** case MT_STOP_BUTTON: if(nNxtButtonPressed==BT_ENTER) { int static TimeDif; TimeDif=time10[T4]; while(nNxtButtonPressed==BT_ENTER){ if(time10[T4]-TimeDif>50) { MV_StopMotors(); ClearSounds(); PlaySound(soundBlip); ESTADO_SET_TARGET(ST_WAIT); break; } } } break; //******** case MT_ACCEL: if(!getSafe(3)) { hogCPU(); HTACreadAllAxes(PORT_ACC,ACCEL[0],ACCEL[1],ACCEL[2]); releaseCPU(); if(abs(ACCEL[ACCEL_AXIS_RAMPA]-ACCEL_Offset)>ACCEL_DELTA){ if (ACCEL_Filter++ > 2){ ACCEL_Rampa = true; ACCEL_Filter = 3; } }else{ if (ACCEL_Filter-- < 2){ ACCEL_Filter = 0; ACCEL_Rampa = false; } } setSafe(3,300); } break; //******** case MT_TOQUE: wait1Msec(1); break; //******** case MT_LL: hogCPU(); LL_Avr = LLreadAverage(PORT_LL); LL_IO = LLreadResult(PORT_LL); releaseCPU(); wait1Msec(5); break; //******** case MT_US: wait10Msec(3); hogCPU(); ReadAllUS_short(PORT_ARD,USwall); releaseCPU(); break; } }