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;
}
}
