View unanswered posts | View active topics It is currently Thu Oct 23, 2014 2:05 pm






Reply to topic  [ 23 posts ]  Go to page 1, 2  Next
Nxt shut off in autonomous. 
Author Message
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Nxt shut off in autonomous.
I am certainly in our classroom for robotics. Trying to get the autonomous to work. And every time that I go to run it the n t will crash and shut off. It works fine for teleop. I can't get Internet on my computer right now, on my phone, and can't get the code up for you to look at if it does have something to do with the code. I am running the same type of template as I had for years robot, new Firmware and that, and it won't work. I have fryer my really old code that doesn't involve the gyro or ir, and only runs off time, and it works for autonomous. Any reason? I can try and get the code up if it has something to do with it.


Fri Jan 06, 2012 7:02 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Nxt shut off in autonomous.
Yep, need your code to see what's going on.


Fri Jan 06, 2012 7:18 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Nxt shut off in autonomous.
Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  HTServo)
#pragma config(Sensor, S1,     ,                    sensorI2CMuxController)
#pragma config(Sensor, S2,     HTMAG,               sensorHiTechnicMagnetic)
#pragma config(Sensor, S3,     gyro,                sensorI2CCustom)
#pragma config(Sensor, S4,     IRSeeker2,           sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  mtr_S1_C1_1,     motorLeft,     tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorRight,    tmotorNormal, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorLift,     tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorFork,     tmotorNormal, openLoop, encoder)
#pragma config(Servo,  srvo_S1_C3_1,    Mag1,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    lift2,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    leftflap3,            tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    rightflap4,           tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    MagR5,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_1,    forkL7,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    forkR8,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    servo9,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_4,    servo10,              tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo11,              tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo12,              tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "C:\Users\Matthew\Documents\Robotics\Code\2011-2012\gyro.h"
#include "C:\Users\Matthew\Documents\Robotics\Code\2011-2012\Encoders12.h"

#define BOUND(n, l, h) (((n) < (l))? (l): ((n) > (h))? (h): (n))

GYRO  g_Gyro;
float g_turnTarget = 0.0;
bool  g_turnEnabled = false;
float g_turnTolerance = 0.5;    //needs to be tuned
float g_turnKp = 50.0;             //needs to be tuned
float g_driveTarget = 0.0;
bool  g_driveEnabled = false;
float g_driveTolerance = 0.3;
float g_driveKp = 50.0;

void SetTurnTarget(float angle)
{
    GyroReset(g_Gyro);
    g_turnTarget = angle;
    g_turnEnabled = true;
}

void TurnTask()
{
    if (g_turnEnabled)
    {
        float error = GyroGetHeading(g_Gyro) - g_turnTarget;
        if (abs(error) > g_turnTolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -70 to 70.
            //
            int turnPower = BOUND((int)(g_turnKp*error), -100, 100);
            motor[motorLeft] = turnPower;
            motor[motorRight] = -turnPower;
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_turnEnabled = false;
            nxtDisplayTextLine(7, "err=%f", error);
        }
        nxtDisplayTextLine(3, "Heading=%f", GyroGetHeading(g_Gyro));
        nxtDisplayTextLine(6, "target=%f,tol=%f", g_turnTarget, g_turnTolerance);
    }
}

float GetDriveDistance()
{
    return (float)(nMotorEncoder[motorLeft] + nMotorEncoder[motorRight])/2.0*INCHES_PER_CLICK;
}

void SetDriveTarget(float distance)
{
    nMotorEncoder[motorLeft] = 0;
    nMotorEncoder[motorRight] = 0;
    GyroReset(g_Gyro);
    g_driveTarget = distance;
    g_driveEnabled = true;
}

void DriveTask()
{
    if (g_driveEnabled)
    {
        float driveErr = g_driveTarget - GetDriveDistance();

        if (abs(driveErr) > g_driveTolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -100 to 100 for drive
            // and -50 to 50 for turn
            //
            int drivePower = BOUND((int)(g_driveKp*driveErr), -100, 100);
            float turnErr = GyroGetHeading(g_Gyro);
            int turnPower = BOUND((int)(g_turnKp*turnErr), -100, 100);
            motor[motorLeft] = BOUND(drivePower + turnPower, -100, 100);
            motor[motorRight] = BOUND(drivePower - turnPower, -100, 100);
            nxtDisplayTextLine(7, "drvErr=%f", driveErr);
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_driveEnabled = false;
        }
        nxtDisplayTextLine(4, "Distance=%f", GetDriveDistance());
   }
}

void processMagnet()
{
    if(SensorValue[HTMAG] > 680) // Magnetic Ball
    {
      servoTarget[Mag1] = 230;
      wait1Msec(1000);
    }
    else if(SensorValue[HTMAG] < 640)// Magnetic Ball
    {
      servoTarget[Mag1] = 230;
      wait1Msec(1000);
    }
    else
    {
      servoTarget[Mag1] = 127;
    }
}

void initializeRobot()
{
  servoTarget[Mag1] = 127;
  servoTarget[leftflap3] = 17;
  servoTarget[rightflap4] = 252;
  servoTarget[forkL7] = 83;
  servoTarget[forkR8] = 83;
  servoTarget[MagR5] = 0;
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
  GyroInit(g_Gyro, gyro, 0);

  return;
}

task main()
{
    int step = 0;

    StopTask(displayDiagnostics);
    eraseDisplay();
    initializeRobot();

    waitForStart(); // Wait for the beginning of autonomous phase.

    while (true)
    {
        GyroTask(g_Gyro);
        motor[motorLift] = 256;
        processMagnet();

        nxtDisplayTextLine(5, "Step=%d", step);
        switch (step)
        {
//*****************************Drive Back Parking Zone*************************************
            case 0:
                // step 0: go forward.
                SetDriveTarget(48.0);
                step++;
                break;

            case 1:
                // step 1: wait for drive to complete.
                if (g_turnEnabled == false)
                {
                    step++;
                }
                break;

            case 2:
                // step 2: Left Flap Out.
                servoTarget[leftflap3] = 135;
                SetDriveTarget(72.0);
                step++;
                break;

            case 3:
                // step 3: wait for drive to complete.
                if (g_driveEnabled == false)
                {
                    step++;
                }
                break;

            case 4:
                // step 2: Left Flap In
                servoTarget[leftflap3] = 0;
                ClearTimer(T1);
                step++;
                break;

            case 5:
                // step 3: wait for drive to complete.
                if (time1[T1] > 500)
                {
                    step++;
                }
                break;

        }

        motor[motorLift] = 0;
        TurnTask();
        DriveTask();
        wait1Msec(10);
    }
}


Fri Jan 06, 2012 9:43 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Nxt shut off in autonomous.
Team2844 wrote:
And every time that I go to run it the n t will crash and shut off.

When you said it crashed, what do you mean exactly? Did RobotC IDE crashed and closed or just the robot crashed. If the NXT crashed, did RobotC give you an error? It normally would highlight the line of code that caused the crash. It would be helpful to know what the error said. I did a quick scan of the code, several things that don't look right:
Code:
while (true)
    {
        GyroTask(g_Gyro);
        motor[motorLift] = 256;
        processMagnet();

Why did you set motorLift to 256? motor value can only be in the range of -100 to 100. What is that code supposed to do? You set it to 256 at the beginning of the while loop and set it to 0 at the end of the loop over and over again. So essentially, you set it to 256 for however long the state machine took to execute one step and then stop the motor for 10 msec over and over again.

Secondly, your processMagnet function is doing a wait for 1 second. It means if you detected a magnet ball, your state machine will stall for 1 second. That defeats the purpose of using a state machine. When using a state machine, you should never have any other loops or using any wait statements. The only exception is the main robot loop and the only wait statement should be the wait in the main loop.

That's the only things I found that don't look right for now. Please give me more info about the crash.


Sat Jan 07, 2012 4:50 am
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Nxt shut off in autonomous.
Ok, after you showed me that, I noticed that it wouldnt work. I took out the process magnet, and the motor lift. What I was trying to do was get the motor to run the front ball lift up front to sweep any balls that may get in front of the robot, and I was trying to run the magnet sensor during the autonomous while still doing the rest of the pieces.

The crash happens to the NXT. I will compile and download the autonomous in RobotC, hit the autonomous ready, and then hit the autonomous running, and the NXT will just crash, and shut right off. I have to then do directly to the NXT and take the battery out and put it back in.... really restarting the NXT. There is no crash information in RobotC. If I touch robotC right after the crash, it will crash the programming on the computer and shut it off. If I wait a minute, it will just log the debugger windows off, and back to the programming.

New code, as I changed a few things, (mostly servo postitions...)
Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  HTServo)
#pragma config(Sensor, S1,     ,                    sensorI2CMuxController)
#pragma config(Sensor, S2,     HTMAG,               sensorHiTechnicMagnetic)
#pragma config(Sensor, S3,     gyro,                sensorI2CCustom)
#pragma config(Sensor, S4,     IRSeeker2,           sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  motorA,          motorA,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorB,          motorB,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          motorC,        tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     motorRight,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     motorLeft,     tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorLift,     tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorFork,     tmotorNormal, openLoop, reversed)
#pragma config(Servo,  srvo_S1_C3_1,    Mag1,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    leftflap2,            tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    rightflap3,           tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    MagG4,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    MagR5,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_1,    forkL7,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    forkR8,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    servo9,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    servo10,              tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo11,              tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo12,              tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "C:\Users\Matthew\Documents\Robotics\Code\2011-2012\gyro.h"
#include "C:\Users\Matthew\Documents\Robotics\Code\2011-2012\Encoders12.h"

#define BOUND(n, l, h) (((n) < (l))? (l): ((n) > (h))? (h): (n))

GYRO  g_Gyro;
float g_turnTarget = 0.0;
bool  g_turnEnabled = false;
float g_turnTolerance = 0.5;    //needs to be tuned
float g_turnKp = 50.0;             //needs to be tuned
float g_driveTarget = 0.0;
bool  g_driveEnabled = false;
float g_driveTolerance = 0.3;
float g_driveKp = 50.0;

void SetTurnTarget(float angle)
{
    GyroReset(g_Gyro);
    g_turnTarget = angle;
    g_turnEnabled = true;
}

void TurnTask()
{
    if (g_turnEnabled)
    {
        float error = GyroGetHeading(g_Gyro) - g_turnTarget;
        if (abs(error) > g_turnTolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -70 to 70.
            //
            int turnPower = BOUND((int)(g_turnKp*error), -100, 100);
            motor[motorLeft] = turnPower;
            motor[motorRight] = -turnPower;
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_turnEnabled = false;
            nxtDisplayTextLine(7, "err=%f", error);
        }
        nxtDisplayTextLine(3, "Heading=%f", GyroGetHeading(g_Gyro));
        nxtDisplayTextLine(6, "target=%f,tol=%f", g_turnTarget, g_turnTolerance);
    }
}

float GetDriveDistance()
{
    return (float)(nMotorEncoder[motorLeft] + nMotorEncoder[motorRight])/2.0*INCHES_PER_CLICK;
}

void SetDriveTarget(float distance)
{
    nMotorEncoder[motorLeft] = 0;
    nMotorEncoder[motorRight] = 0;
    GyroReset(g_Gyro);
    g_driveTarget = distance;
    g_driveEnabled = true;
}

void DriveTask()
{
    if (g_driveEnabled)
    {
        float driveErr = g_driveTarget - GetDriveDistance();

        if (abs(driveErr) > g_driveTolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -100 to 100 for drive
            // and -50 to 50 for turn
            //
            int drivePower = BOUND((int)(g_driveKp*driveErr), -100, 100);
            float turnErr = GyroGetHeading(g_Gyro);
            int turnPower = BOUND((int)(g_turnKp*turnErr), -100, 100);
            motor[motorLeft] = BOUND(drivePower + turnPower, -100, 100);
            motor[motorRight] = BOUND(drivePower - turnPower, -100, 100);
            nxtDisplayTextLine(7, "drvErr=%f", driveErr);
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_driveEnabled = false;
        }
        nxtDisplayTextLine(4, "Distance=%f", GetDriveDistance());
   }
}

void initializeRobot()
{
  servoTarget[Mag1] = 127;
  servoTarget[leftflap2] = 17;
  servoTarget[rightflap3] = 252;
  servoTarget[MagG4] = 175;
  servoTarget[MagR5] = 60;
  servoTarget[forkL7] = 83;
  servoTarget[forkR8] = 83;
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
  GyroInit(g_Gyro, gyro, 0);

  return;
}

task main()
{
    int step = 0;

    StopTask(displayDiagnostics);
    eraseDisplay();
    initializeRobot();

    waitForStart(); // Wait for the beginning of autonomous phase.

    while (true)
    {
        GyroTask(g_Gyro);

        nxtDisplayTextLine(5, "Step=%d", step);
        switch (step)
        {
//*****************************Drive Back Parking Zone*************************************
            case 0:
                // step 0: go forward.
                SetDriveTarget(48.0);
                step++;
                break;

            case 1:
                // step 1: wait for drive to complete.
                if (g_turnEnabled == false)
                {
                    step++;
                }
                break;

            case 2:
                // step 2: Left Flap Out.
                servoTarget[leftflap2] = 135;
                SetDriveTarget(72.0);
                step++;
                break;

            case 3:
                // step 3: wait for drive to complete.
                if (g_driveEnabled == false)
                {
                    step++;
                }
                break;

            case 4:
                // step 2: Left Flap In
                servoTarget[leftflap2] = 0;
                ClearTimer(T1);
                step++;
                break;

            case 5:
                // step 3: wait for drive to complete.
                if (time1[T1] > 500)
                {
                    step++;
                }
                break;

        }

        TurnTask();
        DriveTask();
        wait1Msec(10);
    }
}

All I am trying to do for this autonomous is run forward, while popping out a servo halfwayish through to knock the crates over.


Sat Jan 07, 2012 11:28 am
Profile
Moderator
Moderator
User avatar

Joined: Tue Sep 14, 2010 9:19 pm
Posts: 496
Post Re: Nxt shut off in autonomous.
Does a simple run forward program with nothing else trigger this NXT crash? Sometimes, if the brick experiences a battery disconnect or some similar "trauma" while a program is running, symptoms similar to what you're describing can occur.

_________________
sudo rm -rf /


Sat Jan 07, 2012 12:47 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Nxt shut off in autonomous.
If I run a time autonomous, it doesnt crash. this way of autonomous, does.


Sat Jan 07, 2012 5:57 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Nxt shut off in autonomous.
Okay, now that I am done with FRC kickoff, I looked at your code much more carefully. I see what's going on.
Code:
    while (true)
    {
        GyroTask(g_Gyro);

        nxtDisplayTextLine(5, "Step=%d", step);
        switch (step)
        {
//*****************************Drive Back Parking Zone*************************************
            case 0:
                // step 0: go forward.
                SetDriveTarget(48.0);
                step++;
                break;

            case 1:
                // step 1: wait for drive to complete.
                if (g_turnEnabled == false)
                {
                    step++;
                }
                break;

            case 2:
                // step 2: Left Flap Out.
                servoTarget[leftflap2] = 135;
                SetDriveTarget(72.0);
                step++;
                break;

            case 3:
                // step 3: wait for drive to complete.
                if (g_driveEnabled == false)
                {
                    step++;
                }
                break;

            case 4:
                // step 2: Left Flap In
                servoTarget[leftflap2] = 0;
                ClearTimer(T1);
                step++;
                break;

            case 5:
                // step 3: wait for drive to complete.
                if (time1[T1] > 500)
                {
                    step++;
                }
                break;

        }

        TurnTask();
        DriveTask();
        wait1Msec(10);
    }

In step 0 of your state machine, you set your drive target to 4 ft. But in step 1, you were waiting for the "turn" to complete. You should have been waiting for the drive to complete, not the turn (i.e. g_driveEnabled == false).
That's definitely a bug but it doesn't explain why your NXT is hung. The next thing to do is to use the process of elimination to see which portion of the code is the culprit. For example, assuming there is something wrong in one of the steps in the state machine, you can comment out most of the steps of the state machine and see if NXT is no longer hung. Then you will uncomment one step at a time until the NXT is hung again. Then you know which portion of the code is the cause.


Sat Jan 07, 2012 10:11 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Nxt shut off in autonomous.
I have fixed that, but it is still shuting off. For a slit second you can see the autonomous trying to run, and then the brick freezes and locks up still.


Mon Jan 09, 2012 12:13 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Nxt shut off in autonomous.
Like I said, did you use the process of elimination to figure out which section of the code is the culprit?


Mon Jan 09, 2012 1:11 am
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Nxt shut off in autonomous.
Yes, I have, still the same problem. Would it hahve anything to do with these?

Encoders
Code:
#define PI                      3.14159
#define CLICKS_PER_REVOLUTION   1440.0          //encoder clicks per revolution
#define GEAR_RATIO              (40.0/80.0)     //motor:wheel=40:80 (use your own gear ratio)
#define WHEEL_DIAMETER          4.0  //use your own wheel size
#define WHEEL_CIRCUMFERENCE     (PI*WHEEL_DIAMETER)
#define INCHES_PER_CLICK        (0.005807764)


Gryo
Code:
#if 0
/// Copyright (c) Titan Robotics Club. All rights reserved.
///
/// <module name="gyro.h" />
///
/// <summary>
///     This module contains the library functions for the gyro sensor.
/// </summary>
///
/// <remarks>
///     Environment: RobotC for Lego Mindstorms NXT.
/// </remarks>
#endif

#ifndef _GYRO_H
#define _GYRO_H

#include "C:\Projects\rdpartyrobotcdr-v1.8.1-HT\drivers\HTGYRO-driver.h"

#pragma systemFile

//
// Constants.
//
#define GYROF_USER_MASK         0x00ff
#define GYROF_INVERSE           0x0001
#ifdef HTSMUX_STATUS
  #define GYROF_HTSMUX          0x0080
#endif

#define GYRO_NUM_CAL_SAMPLES    50
#define GYRO_CAL_INTERVAL       10

//
// Macros
//
#define __in
#define __out
#define __inout
#define DEADBAND(n,t)           ((abs(n) > (t))? (n): 0)
#define GyroGetTurnRate(p)      (p.turnRate)
#define GyroGetHeading(p)       (p.heading)

//
// Type definitions.
//
typedef struct
{
    int   sensorID;
    int   gyroFlags;
    int   zeroOffset;
    int   deadBand;
    long  timestamp;
    int   turnRate;
    float heading;
} GYRO;

/**
*  This function calibrates the gyro for zero offset and deadband.
*
*  @param gyro Points to the GYRO structure to be initialized.
*  @param numSamples Specifies the number of calibration samples.
*  @param calInterval Specifies the calibration interval in msec.
*/
void
GyroCal(
    __out GYRO &gyro,
    __in  int numSamples,
    __in  int calInterval
    )
{
    int i;
    int turnRate;
    int min, max;

    gyro.zeroOffset = 0;
    gyro.deadBand = 0;
    min = 1023;
    max = 0;

    for (i = 0; i < numSamples; i++)
    {
#ifdef HTSMUX_STATUS
        turnRate = (gyro.gyroFlags & GYROF_HTSMUX)?
                        HTGYROreadRot((tMUXSensor)gyro.sensorID):
                        HTGYROreadRot((tSensors)gyro.sensorID);
#else
        turnRate = HTGYROreadRot((tSensors)gyro.sensorID);
#endif
        gyro.zeroOffset += turnRate;

        if (turnRate < min)
        {
            min = turnRate;
        }
        else if (turnRate > max)
        {
            max = turnRate;
        }

        wait1Msec(calInterval);
    }

    gyro.zeroOffset /= numSamples;
    gyro.deadBand = max - min;

    return;
}   //GyroCal

/**
*  This function performs the gyro task where it integrates the turn rate
*  into a heading value.
*
*  @param gyro Points to the GYRO structure.
*/
void
GyroTask(
    __inout GYRO &gyro
    )
{
    long currTime;

    currTime = nPgmTime;
#ifdef HTSMUX_STATUS
    gyro.turnRate = (gyro.gyroFlags & GYROF_HTSMUX)?
                        HTGYROreadRot((tMUXSensor)gyro.sensorID):
                        HTGYROreadRot((tSensors)gyro.sensorID);
#else
    gyro.turnRate = HTGYROreadRot((tSensors)gyro.sensorID);
#endif
    gyro.turnRate -= gyro.zeroOffset;
    gyro.turnRate = DEADBAND(gyro.turnRate, gyro.deadBand);
    if (gyro.gyroFlags & GYROF_INVERSE)
    {
        gyro.turnRate = -gyro.turnRate;
    }
    gyro.heading += (float)gyro.turnRate*(currTime - gyro.timestamp)/1000;
    gyro.timestamp = currTime;

    return;
}   //GyroTask

/**
*  This function resets the gyro heading.
*
*  @param gyro Points to the GYRO structure to be reset.
*/
void
GyroReset(
    __out GYRO &gyro
    )
{
    GyroTask(gyro);
    gyro.heading = 0;
    return;
}   //GyroReset

/**
*  This function initializes the gyro sensor.
*
*  @param gyro Points to the GYRO structure to be initialized.
*  @param sensorID Specifies the ID of the gyro sensor.
*  @param gyroFlags Specifies the gyro flags.
*/
void
GyroInit(
    __out GYRO &gyro,
    __in  int sensorID,
    __in  int gyroFlags
    )
{
    gyro.sensorID = sensorID;
    gyro.gyroFlags = gyroFlags & GYROF_USER_MASK;
#ifdef HTSMUX_STATUS
    if (gyro.gyroFlags & GYROF_HTSMUX)
    {
        HTGYROstartCal((tMUXSensor)sensorID);
    }
    else
    {
        HTGYROstartCal((tSensors)sensorID);
    }
#else
    HTGYROstartCal((tSensors)sensorID);
#endif
    GyroCal(gyro, GYRO_NUM_CAL_SAMPLES, GYRO_CAL_INTERVAL);
    gyro.timestamp = nPgmTime;
    GyroReset(gyro);

    return;
}   //GyroInit
#endif  //ifndef _GYRO_H


Joysticks
Code:
////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                    HiTechnic Servo/Motor Controller Device Driver  - UPDATED 1/08/2009//
//
// With the TETRIX system, the PC Controller Station sends messages over Bluetooth to the NXT containing
// current settings of a PC joystick. The joystick settings arrive using the standard NXT BLuetooth
// "message mailbox" facility.
//
// This is a short ROBOTC program to extract the joystick data from a mailbox message and format it
// into data structure that can be easily acccessed by end user programs.
//
// The driver resides in a separate file that can be simply added to any user program with a
// "#include" preprocessor directive. End users should not have to modify this program and can use
// it as is.
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

#if (defined(NXT) || defined(TETRIX)) && (_TARGET == "Robot")
   #pragma autoStartTasks        // Automatically start this task when the main user program starts.
#elif (defined(VEX2) || defined(NXT) || defined(TETRIX)) && (_TARGET == "VirtWorld")
  //Don't auto start tasks
#else
   #error "This driver is not supported on this platform."
#endif

#pragma systemFile

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    Joystick Information Structure
//
//
// Structure containing info from Joystick.
//
// "short" variables are used to prevent conversion errors. For example, negating a byte variable with
// value -128 results in -128 because -128 does not fit in a byte!
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

#if (_TARGET == "Robot")
typedef struct
{
   bool    UserMode;          // Autonomous or Telep-Operated mode
   bool    StopPgm;           // Stop program

   short   joy1_x1;           // -128 to +127
   short   joy1_y1;           // -128 to +127
   short   joy1_x2;           // -128 to +127
   short   joy1_y2;           // -128 to +127
   short   joy1_Buttons;      // Bit map for 12-buttons
   short   joy1_TopHat;       // value -1 = not pressed, otherwise 0 to 7 for selected "octant".

   short   joy2_x1;           // -128 to +127
   short   joy2_y1;           // -128 to +127
   short   joy2_x2;           // -128 to +127
   short   joy2_y2;           // -128 to +127
   short   joy2_Buttons;      // Bit map for 12-buttons
   short   joy2_TopHat;       // value -1 = not pressed, otherwise 0 to 7 for selected "octant".
} TJoystick;
TJoystick joystick;      // User defined variable access

#else
TPCJoystick joystick;
#endif


#if defined(hasJoystickMessageOpcodes)
  intrinsic void getJoystickSettings(TJoystick &joystick)
                         asm(opcdSystemFunctions, byte(sysFuncGetJoysticks),
                               variableRefRAM(joystick));
#endif


////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// "Macro" to get a non-volatile copy of the last joystick settings so
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

#if (_TARGET == "Robot")
#define getJoystickSettings(joystick)    memcpy(joystick, joystickCopy, sizeof(joystick))

bool joy1Btn(int btn)
{   return ((joystick.joy1_Buttons & (1 << (btn - 1))) != 0);  }
bool joy2Btn(int btn)
{   return ((joystick.joy2_Buttons & (1 << (btn - 1))) != 0);  }

#else

#define getJoystickSettings getPCJoystickSettings
bool joy1Btn(int btn)
{   return ((joystick.joy1_Buttons & (1 << (btn - 1))) != 0);  }
#endif


// Code Below Does Not Apply to Virtual/Emulator Robots
#if (defined(NXT) || defined(TETRIX)) && (_TARGET == "Robot")
const TMailboxIDs kJoystickQueueID = mailbox1;
TJoystick joystickCopy;  // Internal buffer to hold the last received message from the PC. Do not use

long ntotalMessageCount = 0;

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                        Receive Messages Task
//
// Dedicated task that continuously polls for a Bluetooth message from the PC containing the joystick
// values. Operaton of this task is nearly transparent to the end user as the earlier "#pragma autoStartTasks"
// statement above will cause it to start running as soon as the user program is started.
//
// The task is very simple. It's an endless loop that continuously looks for a Bluetooth mailbox
// message from the PC. When one is found, it reformats and copies the contents into the internal
// "joystickCopy" buffer.
//
////////////////////////////////////////////////////////////////////////////////////////////////////////
#if (_TARGET == "Robot")
bool bDisconnected = false;
bool bOverrideJoystickDisabling = false;
long nNoMessageCounterLimit = 750;
long nNoMessageCounter = 0;
task readMsgFromPC()
{
   bool bMsgFound;

  TFileIOResult nBTCmdRdErrorStatus;
   const int kMaxSizeOfMessage = 18;
   sbyte tempBuffer[kMaxSizeOfMessage];

   // Initialize setting to default values in case communications with PC is broken.

   //joystickCopy.TeamColor = false;
   joystickCopy.UserMode  = false;
   joystickCopy.StopPgm   = true;

   joystickCopy.joy1_x1 = 0;
   joystickCopy.joy1_y1 = 0;
   joystickCopy.joy1_x2 = 0;
   joystickCopy.joy1_y2 = 0;
   joystickCopy.joy1_Buttons = 0;
   joystickCopy.joy1_TopHat = -1;

   joystickCopy.joy2_x1 = 0;
   joystickCopy.joy2_y1 = 0;
   joystickCopy.joy2_x2 = 0;
   joystickCopy.joy2_y2 = 0;
   joystickCopy.joy2_Buttons = 0;
   joystickCopy.joy2_TopHat = -1;

   bool bTempUserMode,bTempStopPgm;

   while (true)
   {
      // Check to see if a message is available.
      bMsgFound = false;
      bDisconnected = false;
      while (true)
      {
       //
         // There may be more than one message in the queue. We want to get to the last received
         // message and discard the earlier "stale" messages. This loop simply discards all but
         // the last message.
         //
         int nSizeOfMessage;


         nSizeOfMessage = cCmdMessageGetSize(kJoystickQueueID);

         if (nSizeOfMessage <= 0)
         {
            if (!bMsgFound)
            {
               if(nNoMessageCounter > nNoMessageCounterLimit)
               {
                 if(!bOverrideJoystickDisabling)
                 {
                   bTempUserMode = joystickCopy.UserMode;
                   bTempStopPgm = joystickCopy.StopPgm;

                   memset(joystickCopy, 0, sizeof(joystickCopy));

                   joystickCopy.UserMode = bTempUserMode;
                   joystickCopy.StopPgm = bTempStopPgm;
                   joystickCopy.joy1_TopHat = -1;
                   joystickCopy.joy2_TopHat = -1;
                 }
                 bDisconnected = true;
              }
              wait1Msec(4);    // Give other tasks a chance to run
              nNoMessageCounter++;
              continue;        // No message this time. Loop again
            }
            else
            {
              bDisconnected = false;
              nNoMessageCounter = 0;
              break;
           }
            //
            // No more messages available and at least one message found. This is not essential but
            // useful to ensure that we're working with the latest message. We simply discard earlier
            // messages. This is useful because there could be many messages queued and we don't
            // want to work with stale data.
            //
         }

        if (nSizeOfMessage > sizeof(tempBuffer))
          nSizeOfMessage = sizeof(tempBuffer);
        nBTCmdRdErrorStatus = cCmdMessageRead((ubyte)tempBuffer, nSizeOfMessage, kJoystickQueueID);
        nBTCmdRdErrorStatus = nBTCmdRdErrorStatus; //Get rid of info message
        //
        // Repeat loop until there are no more messages in the queue. We only want to process the
        // last message in the queue.
        //
        bMsgFound = true;
      }

     // Once we've reached here, a valid message is available

      hogCPU();   // grab CPU for duration of critical section

      ++ntotalMessageCount;

      joystickCopy.UserMode           = tempBuffer[1];
      joystickCopy.StopPgm            = tempBuffer[2];

      joystickCopy.joy1_x1            = tempBuffer[3];
      joystickCopy.joy1_y1            = tempBuffer[4];
      joystickCopy.joy1_x2            = tempBuffer[5];
      joystickCopy.joy1_y2            = tempBuffer[6];
      joystickCopy.joy1_Buttons       = (tempBuffer[7] & 0x00FF) | (tempBuffer[8] << 8);
      joystickCopy.joy1_TopHat        = tempBuffer[9];

      joystickCopy.joy2_x1            = tempBuffer[10];
     joystickCopy.joy2_y1            = tempBuffer[11];
     joystickCopy.joy2_x2            = tempBuffer[12];
     joystickCopy.joy2_y2            = tempBuffer[13];
     joystickCopy.joy2_Buttons       = (tempBuffer[14] & 0x00FF) | (tempBuffer[15] << 8);
      joystickCopy.joy2_TopHat        = tempBuffer[16];

     joystickCopy.joy1_y1            = -joystickCopy.joy1_y1; // Negate to "natural" position
     joystickCopy.joy1_y2            = -joystickCopy.joy1_y2; // Negate to "natural" position

     joystickCopy.joy2_y1            = -joystickCopy.joy2_y1; // Negate to "natural" position
     joystickCopy.joy2_y2            = -joystickCopy.joy2_y2; // Negate to "natural" position


     releaseCPU(); // end of critical section
   }
}
#endif

///////////////////////////////////////////////////////////////////////////////////////////
//
//                                        displayDiagnostics
//
// THis task will display diagnostic information about a TETRIX robot on the NXT LCD.
//
// If you want to use the LCD for your own debugging use, call the function
// "disableDiagnosticsDisplay()
//
///////////////////////////////////////////////////////////////////////////////////////////


bool bDisplayDiagnostics = false;  // Set to false in user program to disable diagnostic display

void getUserControlProgram(string& sFileName);

#if defined(TETRIX) && (_TARGET == "Robot")

void disableDiagnosticsDisplay()
{
  bDisplayDiagnostics = false;   // Disable diagnostic display
}

task displayDiagnostics()
{
  string sFileName;
  bNxtLCDStatusDisplay = true;
  getUserControlProgram(sFileName);

  while (true)
   {
      if (bDisplayDiagnostics)
      {
        nxtDisplayTextLine(6, "Teleop FileName:");
        nxtDisplayTextLine(7, sFileName);

        getJoystickSettings(joystick);                   //Update variables with current joystick values

         if (joystick.StopPgm)
           nxtDisplayCenteredTextLine(1, "Wait for Start");
         else if (joystick.UserMode)
            nxtDisplayCenteredTextLine(1, "TeleOp Running");
         else
            nxtDisplayCenteredTextLine(1, "Auton Running");

         if ( externalBatteryAvg < 0)
            nxtDisplayTextLine(3, "Ext Batt: OFF");       //External battery is off or not connected
         else
            nxtDisplayTextLine(3, "Ext Batt:%4.1f V", externalBatteryAvg / (float) 1000);

         nxtDisplayTextLine(4, "NXT Batt:%4.1f V", nAvgBatteryLevel / (float) 1000);   // Display NXT Battery Voltage

         nxtDisplayTextLine(5, "FMS Msgs: %d", ntotalMessageCount);   // Display Count of FMS messages
      }

      wait1Msec(200);
  }
}

///////////////////////////////////////////////////////////////////////////////////////////
//
//                                        getUserControlProgram
//
// This function returns the name of the TETRIX User Control program. It reads the file
// "FTCConfig.txt" and builds the name of the file from the contents.
//
// Note that the filename includes the ".rxe" (robot executable file) file extension.
//
///////////////////////////////////////////////////////////////////////////////////////////
#endif
const string kConfigName = "FTCConfig.txt";

void getUserControlProgram(string& sFileName)
{
  byte   nParmToReadByte[2];
  int    nFileSize;
   TFileIOResult nIoResult;
   TFileHandle hFileHandle;

  sFileName = "";
  nParmToReadByte[1] = 0;
  hFileHandle = 0;
  OpenRead(hFileHandle, nIoResult, kConfigName, nFileSize);
  if (nIoResult == ioRsltSuccess)
  {
    for (int index = 0; index < nFileSize; ++index)
    {
      ReadByte(hFileHandle, nIoResult,  nParmToReadByte[0]);
      strcat(sFileName, nParmToReadByte);
    }

    //
    // Delete the ".rxe" file extension
    //
    int nFileExtPosition;

    nFileExtPosition = strlen(sFileName) - 4;
    if (nFileExtPosition > 0)
      StringDelete(sFileName, nFileExtPosition, 4);
  }
  Close(hFileHandle, nIoResult);
  return;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    waitForStart
//
// Wait for the start of either the autonomous or tele-op phase. User program is running on the NXT
// but the phase has not yet started. The FMS (Field Management System) is continually (every 50 msec)
// sending information to the NXT. This program loops getting the latest value of joystick settings.
// When it finds that the FMS has started the  phase, it immediately returns.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void waitForStart()
{
  while (true)
  {
    getJoystickSettings(joystick);
    if (!joystick.StopPgm)
      break;
  }
  return;
}



#endif


Mon Jan 09, 2012 10:16 am
Profile
Site Admin
Site Admin

Joined: Wed Jan 24, 2007 10:42 am
Posts: 613
Post Re: Nxt shut off in autonomous.
You should send your complete code as an attachment (includes and all) to support@robotc.net - We'll be able to better diagnose the issue having all of the files in one place.

Thanks!

_________________
Timothy Friez
ROBOTC Developer - SW Engineer
tfriez@robotc.net


Mon Jan 09, 2012 4:18 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Nxt shut off in autonomous.
Team 2844,
Please do what tfriez recommended to send the entire code to support@robotc.net. I am very busy these few weeks preparing for the FTC state championship as well as the start of FRC season. So I may not be able to respond to your posts quickly.


Mon Jan 09, 2012 4:33 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Nxt shut off in autonomous.
Thank you. Sent it. Thanks for trying to help MHTS, I know how you feel, I have the same problem frc and ftc.


Tue Jan 10, 2012 12:06 am
Profile
Site Admin
Site Admin

Joined: Wed Jan 24, 2007 10:42 am
Posts: 613
Post Re: Nxt shut off in autonomous.
So I tried to run your code (that you sent us via Pastebin) - I do not have a Gyro sensor (I do have the rest) and I couldn't replicate the issue. I was able to download the code and have it run without locking up the NXT.

I've attached what I compiled and tested it with 3.04 and our unreleased 3.05 and I couldn't get it to crash the NXT at all.

Maybe if Xander is out there he can try it for me and let me know if he runs into the same issues. I grabbed a copy of the 2.4 gyro driver of Xanders - You might want to try what I've attached as a ZIP and see if it works for you.


Attachments:
FTC Bug.zip [8.21 KiB]
Downloaded 400 times

_________________
Timothy Friez
ROBOTC Developer - SW Engineer
tfriez@robotc.net
Tue Jan 10, 2012 11:11 am
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 23 posts ]  Go to page 1, 2  Next

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.