//------------
//=================================================
// Task to handle the gyro and the other sensors hooked up to the sensor mux
//=================================================
task gyro()
{
float currDir = 0.0; float prevDir = 0.0;
long currtime,prevtime;
ir_mux_status=HTSMUXreadPowerStatus(IR_MUX); // read the sensor multiplexor status
gyro_mux_status=HTSMUXreadPowerStatus(GYRO_MUX); // read the sensor multiplexor status
while (ir_mux_status || gyro_mux_status) // check good battery power on both muxes
{
PlayTone(750,25);
wait1Msec(500);
}
wait1Msec(300);
for (int i=0;i<5;i++) // check if there is too much spread in the data
{
if (gyro_noise>10)
{
PlayTone (250,25);
wait1Msec(500);
}
}
calibrate = 2;
prevtime = nPgmTime;
while(true)
{
currtime=nPgmTime;
rawgyro = (float)HTGYROreadRot(HTGYRO);
constHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000;
relHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000;
prevtime = currtime;
wait1Msec(1);
prevDir = currDir;
}
}
task main()
{
nMotorEncoder[motorRight] = 0;
nMotorEncoder[motorLeft] = 0;
motor[motorLeft] = 20;
motor[motorRight] = 20;
while ( SensorValue[IRsensor] != 6)
{
wait1Msec(10);
nxtDisplayCenteredTextLine(3, "Sensor Value: %d", SensorValue[IRsensor]);
}
motor[motorLeft] = 0;
motor[motorRight] = 0;
PlayTone(736, 50);
wait1Msec(1000);
//Now go for x amount of encoder counts//
motor[motorLeft] = 20;
motor[motorRight] = 20;
while ( nMotorEncoder[motorLeft] < (360 * 4 * 9.5))
{
wait10Msec(1);
}
motor[motorLeft] = 0;
motor[motorRight] = 0;
PlayTone(736, 50);
wait1Msec(1000);
}
//--------------------------------------------------------------------------------------------------
static void* PlayDtmfThread
(
void* contextPtr
)
{
DtmfThreadCtx_t *threadCtxPtr = (DtmfThreadCtx_t*) contextPtr;
size_t dtmfLen = strlen(threadCtxPtr->dtmfPtr);
uint32_t i;
for (i=0 ; i< dtmfLen ; i++)
{
LE_INFO("Play digit.%c", threadCtxPtr->dtmfPtr[i]);
PlayTone(threadCtxPtr->pipefd[1], threadCtxPtr->sampleRate,
Digit2LowFreq(threadCtxPtr->dtmfPtr[i]), DTMF_AMPLITUDE,
Digit2HighFreq(threadCtxPtr->dtmfPtr[i]), DTMF_AMPLITUDE,
threadCtxPtr->duration);
if (threadCtxPtr->pause)
{
// Play silence
PlayTone(threadCtxPtr->pipefd[1], threadCtxPtr->sampleRate,
0, 0,
0, 0,
threadCtxPtr->pause);
}
}
return NULL;
}
// Allow the user to calibrate the scales
void calibrateScales()
{
int calibrateWeight = 0;
eraseDisplay();
nxtDisplayCenteredTextLine(0, "GlideWheel-AS");
nxtDisplayCenteredTextLine(2, "Place the object");
nxtDisplayCenteredTextLine(3, "on the scales");
nxtDisplayCenteredTextLine(4, "and press");
nxtDisplayCenteredTextLine(5, "[enter]");
nxtDisplayCenteredTextLine(6, "to calibrate");
while (nNxtButtonPressed != kEnterButton) EndTimeSlice();
debounce();
eraseDisplay();
calibrateWeight = weighObject();
nxtDisplayCenteredTextLine(0, "GlideWheel-AS");
nxtDisplayCenteredTextLine(2, "Enter the weight");
nxtDisplayCenteredTextLine(3, "in grams");
nxtDisplayCenteredTextLine(7, "- OK +");
while (true)
{
nxtDisplayCenteredBigTextLine(5, "%d", calibrateWeight);
switch(nNxtButtonPressed)
{
case kLeftButton: PlayTone(500,10); calibrateWeight--; calibrateWeight = max2(0, calibrateWeight); break;
case kRightButton: PlayTone(1000,10); calibrateWeight++; break;
case kEnterButton: PlayTone(1500,10);gramsPerUnit = (float)calibrateWeight / (float)MSANGreadRaw(MSANG); eraseDisplay(); return;
}
wait1Msec(20);
debounce();
}
}
int StallCode(tMotor motorSentTo, int wantedPower)
{
int motorIndex; //index value for the arrays we are storing values in.
int direction = 0;
switch(motorSentTo) //which motor power is being sent to
{
case LeftMotor: // This is the name of one of the motors as referenced in the configuraiton.
motorIndex = 0;
break;
case RightMotor:
motorIndex = 1;
break;
/*case ForkLift:
motorIndex = 2;
break;*/
default:
break;
}
if (abs(wantedPower) < deadZone) // Power below threshold, mark as stopped.
direction = 0;
else
direction = (wantedPower < 0) ? -1 : 1;
if (direction == 0 || lastDirection[motorIndex] != direction) // Stopped or changed direction. Allow whatever power desired this time.
{
lastDirection[motorIndex] = direction;
timeLastSigMove[motorIndex] = time1[T1];
encLastSigMove[motorIndex] = nMotorEncoder[motorSentTo];
return wantedPower;
}
lastDirection[motorIndex] = direction;
if ( abs(encLastSigMove[motorIndex] - nMotorEncoder[motorSentTo]) > sigMove) // Moved far enough to be considered significant, mark
{
timeLastSigMove[motorIndex] = time1[T1];
encLastSigMove[motorIndex] = nMotorEncoder[motorSentTo];
return wantedPower;
}
if ( (time1[T1] - timeLastSigMove[motorIndex]) > wayTooLong ) // Time since last significant move too long, stalled
{
PlayTone(650,4);
return 0;
}
if ( (time1[T1] - timeLastSigMove[motorIndex]) > tooLong ) // Time since last significant move too long, stalled
{
PlayTone(365,4);
return wantedPower / 2;
}
return wantedPower; // Haven’t moved far enough yet to be significant but haven’t timed out yet
}
/* Shooter speed decrease feedback */
task soundSpeedDown(){
// 140 = Tempo
// 5 = Default octave
// Quarter = Default note length
// 10% = Break between notes
//
PlayTone( 932, 19); wait1Msec( 214); // Note(A#, Duration(Eighth))
PlayTone( 698, 19); wait1Msec( 214); // Note(F, Duration(Eighth))
}
task main()
{
nxtDisplayCenteredTextLine(3, "Servos");
wait1Msec(750);
servo [scoopCover] = 230;
servo [leftLatch] = 252;
servo [rightLatch] = 10;
eraseDisplay();
wait1Msec(750);
nxtDisplayCenteredTextLine(3, "Lift 1");
nxtDisplayCenteredTextLine(5, "Orange stops");
while (nNxtButtonPressed != 3)
{
motor [leftLift] = -50;
}
while (nNxtButtonPressed != (-1))
{
}
motor [leftLift] = 0;
eraseDisplay();
wait1Msec(750);
nxtDisplayCenteredTextLine(3, "Lift 2");
nxtDisplayCenteredTextLine(5, "Orange stops");
while (nNxtButtonPressed != 3)
{
motor [rightLift] = -50;
}
while (nNxtButtonPressed != (-1))
{
}
motor [rightLift] = 0;
eraseDisplay();
wait1Msec(750);
nxtDisplayCenteredTextLine(3, "Arm");
nxtDisplayCenteredTextLine(5, "Orange stops");
while (nNxtButtonPressed != 3)
{
motor [arm] = -15;
}
if (nNxtButtonPressed == 3) {
while (nNxtButtonPressed != (-1))
{
}
motor [arm] = 0;
eraseDisplay();
}
nxtDisplayCenteredTextLine(3, "Stored.");
PlayTone (1000, 70);
wait1Msec (1000);
PlayTone (1000, 70);
wait1Msec (1000);
}
// All done tones.
void Cocacola()
{
// 125 = Tempo
// 5 = Default octave
// Quarter = Default note length
// 10% = Break between notes
//
PlayTone( 988, 22); wait1Msec( 240); // Note(E6, Duration(Eighth))
PlayTone( 1320, 22); wait1Msec( 240); // Note(A6, Duration(Eighth))
PlayTone( 1109, 43); wait1Msec( 480); // Note(F#6)
PlayTone( 880, 43); wait1Msec( 480); // Note(D6)
PlayTone( 0, 86); wait1Msec( 960); // Note(Rest, Duration(Half))
return;
}
void swapJoys()
{
motor[leftMotor] = 0;
motor[rightMotor] = 0;
joySwap = !joySwap;
PlayTone(784, 50);
}
task NavigateByTachometer()
{
tnFlags flags = tnFlagsReadOnly;
ResetAllTachoCounts(flags.leftMotor);
ResetAllTachoCounts(flags.rightMotor);
unsigned int time;
unsigned int delta;
while(true)
{
TextOut(0, flags.posLine,
"P: " + v2AsString(flags.position) +
" " , DRAW_OPT_NORMAL);
TextOut(0, flags.headLine, "H: " +
NumToStr(flags.heading) +
" " , DRAW_OPT_NORMAL);
time = CurrentTick();
tnFlags temp = __updateHeadingPosition(flags, time + 50);
if(temp.convergenceFailure)
PlayTone(2000, 100);
else
flags = temp;
delta = CurrentTick() - time;
tnFlagsReadOnly = flags;
if(delta < 25)
Wait(25 - delta);
}
}
/*turn on the spot if the timer expires, return false if it could not find another branch*/
bool turnOnSpot(short target, bool greyPatch){
float cur_compass = compass();
//start rotating
stop();
control(-rotateSpeed, rotateSpeed);
while(compass() - cur_compass < 390){
float diff = compass() - cur_compass;
if(SensorValue[Light] < target && (diff < 180 || diff > 250)){ // ignore the original branch
time1[T1] = 0;
PlayTone(1175,20);
stop();
adjustCount++;
return true; // return true if it finds another branch
}
}
// on the endpoint
stop();
if(!greyPatch){ // if not yet detect a grey patch, turn back
control(-rotateSpeed, rotateSpeed);
while(SensorValue[Light] > target){}
stop();
}
return false;
}
void navigateToWaypoint(float xtarget, float ytarget)
{
while(!atTarget(xtarget, x) || (!atTarget(ytarget, y)))
{
float distance = calcDistance(xtarget, ytarget);
float angle = calcAngle(xtarget, ytarget);
float driveDistance = 0; //initialise
if (distance < stepDistance)
{
driveDistance = distance;
}
else
{
driveDistance = stepDistance;
}
// co-ordinate system is different so cos and sin are swapped - take from current location not origin
float tempWaypointX = x + (driveDistance * cos(angle));
float tempWaypointY = y + (driveDistance * sin(angle));
driveToWaypoint(tempWaypointX, tempWaypointY);
monteCarlo();
eraseDisplay();
drawMap();
drawParticles();
wait1Msec(100);
}
PlayTone(784, 15);
}
//************************
// Segue Linha
//************************
void F_STATE_LINHA()
{
MTASK_SET_RUN(MT_STOP_BUTTON);
int Error = 0;
while(ESTADO_IS_CURRENT())
{
//=====================================
int Error = LLreadAverage(S1)-45;
if(Error ==-45){
Error = 0;
PlayTone(200,1);
}
nxtDisplayStringAt(20, 20, "%i",Error);
motor[motorA]=-20 - Error;
motor[motorC]=-20 + Error;
//Se detectou rampa com acceleracao, sobe a rampa
if(ACCEL_Rampa)ESTADO_SET_TARGET(ST_SOBE);
//=====================================
}
}
void initialize()
{
motor[frontLeftWheel] = 0;
motor[frontRightWheel] = 0;
motor[backLeftWheel] = 0;
motor[backRightWheel] = 0;
motor[leftArm] = 0;
motor[rightArm] = 0;
nMotorEncoder[rightArm] = 0;
//nMotorEncoderTarget[rightArm] = 10;
servo[leftClaw] = LCLAWOPEN;
servo[rightClaw] = RCLAWOPEN;
string BatteryLevel = externalBatteryAvg;
nxtDisplayCenteredBigTextLine (3, BatteryLevel);
if(externalBatteryAvg<13000){
PlayTone(500,10*(13000-externalBatteryAvg));
}
wait1Msec(5000);
nxtDisplayCenteredBigTextLine (5, "Teleop Running.");
}
void SG(int samples){
PlayTone(300,10);
//De quantos em quantos valores ele ira ler
if(samples<=100){
SG_jump=1;
}else{
SG_jump=2;
}
//Configs: Jump, yScale, EXIT
eraseDisplay();
SG_ySpace=0;
SG_yScale=2;
SG_update=0;
SG_exit=0;
SG_pos=1;
SG_xSpace=1;
while(SG_exit==0){
SG_Button();
if(SG_update==0){
eraseDisplay();
for(int i=1;i<100 && i<samples/SG_jump;i++){
nxtDrawRect(i+SG_xSpace, SG_ySpace+AUlt[(i-1)*SG_jump]/SG_yScale,i+1+SG_xSpace,SG_ySpace+AUlt[i*SG_jump]/SG_yScale);
}
SG_Config(0);
SG_update=1;
}
}
}
void init()
{
bSmartDiagnostics = true; //true to enable smart diagnostic screen
bCompetitionMode = true; //true to enable competition mode
displaySplash("Mecanum Bot", "", true);
bool ok = false;
while(!ok)
{
const int testcount = 2;
bool test[testcount] = {
errorcheck(1,0,1,MOTORCON),
errorcheck(1,0,2,MOTORCON)};
string desc[testcount] = {"MC1-1","MC1-2"};
ok = error_display(test,desc,testcount);
if (!ok) {
PlayTone(440, 50);
if (test[0] == false && test[1] == false){
nxtDisplayCenteredTextLine(7, "Reboot MC!");
}
}
else { ClearSounds(); }
}
eraseDisplay();
gyro_init(HTGYRO);
wait1Msec(50);
nxtbarOn();
return;
}
void ToneClass::RandomTone(DigitalClass::DigitalPortEnum digitalPort, unsigned char lengthMs, int numCount){
srand(time(NULL));
for (int i = 0; i <= numCount; i++){
int num3 = rand() % NoteCount;
PlayTone(digitalPort, (NoteEnum)num3, lengthMs);
}
}
task main()
{
eraseDisplay();
bNxtLCDStatusDisplay = true; // Enable top status line display
wifi_startup(); // This is where the work is done.
PlaySound(soundBeepBeep); // Make some noise when we're connected.
PlayTone(500, 100); // Make some noise when we're connected.
}
void beep() {
// 100 = Tempo
// 5 = Default octave
// Quarter = Default note length
// 10% = Break between notes
PlayTone( 932, 27); wait1Msec( 75); // Note(A#, Duration(32th))
return;
}
void boop() {
// 100 = Tempo
// 6 = Default octave
// Quarter = Default note length
// 10% = Break between notes
PlayTone( 1047, 27); wait1Msec( 75); // Note(F, Duration(32th))
return;
}
//======================================================
// diagnostic beeps
//======================================================
void diagnosticBeeps(int number)
{
for(int i=0;i<number;i++)
{
PlayTone(300,50);
wait1Msec(800);
}
}
//************************
// Sobe a Rampa
//************************
void F_STATE_SOBE()
{
PlayTone(500,10);
MTASK_SET_RUN(MT_ACCEL,MT_STOP_BUTTON);
int USminDist = 999, USdist,USdelta,Lado1,Lado2;
float Error = 0, vI = 0, vP = 0, vD = 0, kI = 0.000001, kP = 2, kD = 0;
tByteArray USwall;
time1[T1]=0;
while(ESTADO_IS_CURRENT())
{
//=====================================
// Le arduino para pingar os ultrasons
wait1Msec(30);
ReadAllUS_short(PORT_ARD,USwall);
// Filtra dados
//// Seta as variaveis locais como locais
//// para nao dar bug (alterar o valor na outra task)
Lado1=USwall[0];
Lado2=USwall[2];
//// Acha a menor distancia que o robo fica das paredes
if(Lado1+Lado2<USminDist)USminDist=USdist;
//// Filtra paredes (existe ou nao)
if(Lado1>150)Lado1=USminDist-Lado2;
if(Lado2>150)Lado2=USminDist-Lado1;
// Calcula o Error
vD = time1[T1] * ((Lado1 - Lado2) - vP);
vP = (Lado1 - Lado2);
vI += time1[T1] * vP;
if(vI>30) vI = 30;
if(vI<-30) vI = -30;
Error = vP*kP+
vI*kI-
vD*kD;
time1[T1]=0;
// Aplica erro aos motores
motor[MA]=80 - Error;
motor[MC]=80 + Error;
//Se saiu da rampa com acceleracao, entra na sala
if(!ACCEL_Rampa)ESTADO_SET_TARGET(ST_ENTRA);
//=====================================
}
}
task main ()
{
while (1)
{
nxtDisplayCenteredBigTextLine(3, "%d", SensorValue [magnetron]);
PlayTone(SensorValue [magnetron], 25 /*50*/);
wait1Msec(500);
}
}
int getSeeker()
{
int _dirDC = 0;
int _dirAC = 0;
int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
// set the DSP to the new mode
if ( ! HTIRS2setDSPMode(seeker, DSP_1200))
return -1; // Sensor initialized
// Read the current non modulated signal direction
_dirDC = HTIRS2readDCDir(seeker);
if (_dirDC < 0)
return -1; // I2C read error occurred
// read the current modulated signal direction
_dirAC = HTIRS2readACDir(seeker);
if (_dirAC < 0)
return -1; // I2C read error occurred
// Read the individual signal strengths of the internal sensors
// Do this for both unmodulated (DC) and modulated signals (AC)
if (!HTIRS2readAllDCStrength(seeker, dcS1, dcS2, dcS3, dcS4, dcS5))
return -1; // I2C read error occurred
if (!HTIRS2readAllACStrength(seeker, acS1, acS2, acS3, acS4, acS5 ))
return -1; // I2C read error occurred
count ++;
writeDebugStreamLine("D %d %d", count, _dirAC);
//writeDebugStreamLine("0 %d %d", dcS1, acS1);
//writeDebugStreamLine("1 %d %d", dcS2, acS2);
//writeDebugStreamLine("2 %d %d", dcS3, acS3);
//writeDebugStreamLine("3 %d %d", dcS4, acS4);
//writeDebugStreamLine("4 %d %d", dcS5, acS5);
if(_dirAC == 2)// && acS2 < 50 && acS4 < 50) // make sure that the beacon is right in front of the sensor
{
static int debugPrinted = 0;
PlayTone(440<<2, 10);
if (!debugPrinted){
debugPrinted = true;
writeDebugStreamLine("Played sound at %d degrees", gyroVal);
//basketNumber = degreesToBasket(gyroVal);
}
return 1;
}
else if(_dirAC == 3)
return 2;
else if(_dirAC == 4)
return 3;
else
{
return -1*_dirAC;
}
}
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 main()
{
if(getChoice("What pitch?", "Low", "High") == 0)
{
PlayImmediateTone(440, 100);
}else{
PlayTone(880, 100);
}
wait1Msec(1000);
}
//------------
//=================================================
// Task to handle the gyro and the other sensors hooked up to the sensor mux
//=================================================
task gyro()
{
float currDir = 0.0; float prevDir = 0.0;
long currtime,prevtime;
ir_mux_status=HTSMUXreadPowerStatus(IR_MUX); // read the sensor multiplexor status
gyro_mux_status=HTSMUXreadPowerStatus(GYRO_MUX); // read the sensor multiplexor status
while (ir_mux_status || gyro_mux_status) // check good battery power on both muxes
{
PlayTone(750,25);
wait1Msec(500);
}
// testvalue = HTGYROreadRot(HTGYRO); // check gyro value appears meaningful
// while (abs(testvalue)>50)
// {
// PlayTone(500,25);
// wait1Msec(500);
// }
//while(calibrate != 1){};
wait1Msec(300);
//HTGYROstartCal(HTGYRO);
//drift = MyHTCal(gyroCalTime*1000);
for (int i=0;i<5;i++) // check if there is too much spread in the data
{
if (gyro_noise>10)
{
PlayTone (250,25);
wait1Msec(500);
}
}
calibrate = 2;
prevtime = nPgmTime;
while(true)
{
currtime=nPgmTime;
rawgyro = (float)HTGYROreadRot(HTGYRO);
constHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000;
relHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000;
prevtime = currtime;
wait1Msec(1);
prevDir = currDir;
}
}
void findLine( int thresh, int power )
{
int mult;
if( thresh > 0 )
mult = 1;
else
mult = -1;
PlayTone(1000, 20);
while(SensorValue(S2) > abs(thresh) )
{
motor[motorC] = power * mult;
motor[motorB] = power * mult;
}
PlayTone(1500, 20);
motor[motorC] = 0;
motor[motorB] = 0;
}
//=====================================================================
void StartBeeps()
{
int c;
ClearScreen();
TextOut(0, LCD_LINE1, "Bluetooth-Segway");
TextOut(20, LCD_LINE3, "Starte in");
TextOut(0, LCD_LINE5, "Stelle mich");
TextOut(0, LCD_LINE6, "bitte auf.");
// Play warning beep sequence to indicate balance about to start
for (c=5; c>0;c--) {
NumOut(47, LCD_LINE4, c);
PlayTone(440,100);
Wait(1000);
}
NumOut(47, LCD_LINE4, 0);
PlayTone(440,1000);
Wait(1000);
}
task main ()
{
int speed = 35;
PlayTone(784, 15);
turnTask(180, speed);
wait1Msec(1000);
PlayTone(784, 15);
turnTask(-180, speed);
wait1Msec(1000);
PlayTone(784, 15);
turnTask(360, speed);
wait1Msec(1000);
PlayTone(784, 15);
turnTask(-360, speed);
wait1Msec(1000);
}
