View unanswered posts | View active topics It is currently Wed Apr 23, 2014 12:24 pm






Reply to topic  [ 1 post ] 
EV3 Project: GyroBoy 
Author Message
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post EV3 Project: GyroBoy
In the Education version of the LabVIEW software, it included the build instructions and code for the "Gyro Boy" project. I built it and tried it out. It's pretty cool. But since I don't really know LabVIEW and RobotC doesn't support EV3 yet, I decided to port the Gyro Boy code to RobotC on an NXT. I built a similar Gyro Boy with NXT without the arms (i.e. no color sensor, no sonar sensor and no arm motor) and no touch sensor either. It seems the code is trying to balance but it crashes very quickly. I am sure I need to tune a lot of the control parameters to get it to work reliably. Unfortunately, FTC season is starting. I probably won't have time to dig too deep into it. If anybody on the forum wants to give it a try, here is the code. Please let me know if I "misinterpret" the LabVIEW code in my translation.
Code:
#pragma config(Sensor, S1,     colorSensor,    sensorCOLORFULL)
#pragma config(Sensor, S2,     gyroSensor,     sensorI2CHiTechnicGyro)
#pragma config(Sensor, S3,     touchSensor,    sensorTouch)
#pragma config(Sensor, S4,     sonarSensor,    sensorSONAR)
#pragma config(Motor,  motorA,          rightMotor,    tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorB,          armMotor,      tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          leftMotor,     tmotorNXT, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//#define _USE_BHV

float   mPos;   //Motor position (degrees)
int     mSum;   //Sum of left and right encoders
int     mD;     //Encoder changes for the loop interval
int     mDP1;   //Encoder changes for t-1
int     mDP2;   //Encoder changes for t-2
int     mDP3;   //Encoder changes for t-3
float   mSpd;   //Motor speed
int     Cdrv;   //Drive speed (degrees/s)
int     Cstr;   //Turn speed
int     cLo;    //Loop counter
float   gSpd;   //Gyro turn rate
float   gAng;   //Gyro angle
bool    ok;     //Okay to quit
int     pwr;    //Motor power
int     st;     //State
float   gOS;    //Gyro zero offset
float   tInt;   //Loop time interval

#ifdef _USE_BHV

/**
 * This function rotates the motor with the given power to the given angle.
 */
void RotateMotor(tMotor motorID, int power, int angle)
{
    nMotorEncoder[motorID] = 0;
    while (abs(nMotorEncoder[motorID]) < angle)
    {
        motor[motorID] = power;
    }
    motor[motorID] = 0;
}   //RotateMotor

/**
 * This task monitors for state changes, the color and sonar sensors.
 */
task BHV()
{
    st = 0;
    while (true)
    {
        switch (st)
        {
            default:
            case 0:
                //
                // Stop.
                //
                Cdrv = 0;
                Cstr = 0;
                break;

            case 1:
                //
                // Launch: Move forward for 4 seconds and stop.
                //         Then proceed to next state.
                //
                Cdrv = 40;
                wait1Msec(4000);
                Cdrv = 0;
                PlayTone(1000, 10);
                st = 2;
                break;

            case 2:
                //
                // Monitor color sensor for instructions.
                //
                switch (SensorValue[colorSensor])
                {
                    case REDCOLOR:
                        //
                        // Stop.
                        //
                        PlayTone(2000, 10);
                        Cdrv = 0;
                        Cstr = 0;
                        break;

                    case GREENCOLOR:
                        //
                        // Forward.
                        //
                        PlayTone(2000, 10);
                        Cdrv = 150;
                        Cstr = 0;
                        break;

                    case BLUECOLOR:
                        //
                        // Turn right.
                        //
                        PlayTone(2000, 10);
                        Cstr = 70;
                        break;

                    case YELLOWCOLOR:
                        //
                        // Turn left.
                        //
                        PlayTone(2000, 10);
                        Cstr = -70;
                        break;

                    case WHITECOLOR:
                        //
                        // Backup.
                        //
                        PlayTone(2000, 10);
                        Cdrv = -75;
                        break;

                    default:
                        break;
                }

                if (SensorValue[sonarSensor] < 25)
                {
                    //
                    // Detected obstacle in front:
                    // - Back up a little.
                    // - Make arms flailing.
                    // - Randomly turn left or right for 4 seconds.
                    // - Resume previous command.
                    //
                    Cstr = 0;
                    int oldDr = Cdrv;
                    Cdrv = -10;
                    RotateMotor(armMotor, 30, 30);
                    RotateMotor(armMotor, -30, 60);
                    RotateMotor(armMotor, 30, 30);
                    if (rand()%2)
                    {
                        Cstr = 70;
                    }
                    else
                    {
                        Cstr = -70;
                    }
                    wait1Msec(4000);
                    PlayTone(2000, 10);
                    Cstr = 0;
                    Cdrv = oldDr;
                }
                break;
        }
        wait1Msec(100);
    }
}   //task BHV
#endif

/**
 * This function resets all variables and motor states.
 */
void RST()
{
    //
    // Reset leftMotor
    //
    motor[leftMotor] = 0;
    nMotorEncoder[leftMotor] = 0;

    //
    // Reset rightMotor
    //
    motor[rightMotor] = 0;
    nMotorEncoder[rightMotor] = 0;

//    gyroReset(2);
    ClearTimer(T2);

    mPos = 0.0;
    mSum = 0;
    mD = 0;
    mDP1 = 0;
    mDP2 = 0;
    mDP3 = 0;
    Cdrv = 0;
    Cstr = 0;
    cLo = 0;
    gAng = 0.0;
    ok = false;
    pwr = 0;
    st = 0;
}   //RST

/**
 * This function calibrates the gyro zero offset.
 */
void calGyroOffset()
{
    int gMn;
    int gMx;
    float gSum;
    int gValue;

    do
    {
        gMn = 1000;
        gMx = -1000;
        gSum = 0.0;
        for (int i = 0; i < 200; i++)
        {
            gValue = SensorValue[gyroSensor];
            gSum += gValue;
            if (gValue > gMx)
            {
                gMx = gValue;
            }
            if (gValue < gMn)
            {
                gMn = gValue;
            }
            wait1Msec(4);
        }
    } while (gMx - gMn > 2);
    gOS = gSum/200;
}   //calGyroOffset

/**
 * This function calculates the average time interval of a loop.
 */
void GT()
{
    if (cLo == 0)
    {
        //
        // Let's say the first loop interval is 14ms.
        //
        tInt = 0.014;
        ClearTimer(T1);
    }
    else
    {
        //
        // time interval = elapsed time/loop counter
        //
        tInt = (float)time1[T1]/1000.0/cLo;
    }
    cLo++;
}   //GT

/**
 * This function calculates the adjusted gyro turn rate and the integrated
 * angle.
 */
void GG()
{
    //
    // Apply filter to zero offset to compensate for drift.
    //
    int gRawRate = SensorValue[gyroSensor];
    gOS = 0.0005*gRawRate + 0.9995*gOS;
    gSpd = gRawRate - gOS;
    gAng += gSpd*tInt;
}   //GG

/**
 * This function calculates the motor speed and position.
 */
void GM()
{
    int mSumPrev = mSum;
    mSum = nMotorEncoder[leftMotor] + nMotorEncoder[rightMotor];
//    int mDiff = nMotorEncoder[rightMotor] - nMotorEncoder[leftMotor];
    mD = mSum - mSumPrev;
    mPos += mD;
    mSpd = ((float)(mDP3 + mDP2 + mDP1 + mD)/4.0)/tInt;
    mDP3 = mDP2;
    mDP2 = mDP1;
    mDP1 = mD;
}   //GM

/**
 * This function calculates the motor power according to the motor and gyro
 * readings.
 */
void EQ()
{
    //
    // mPos = currPos - targetPos
    //
    mPos -= tInt*Cdrv;
    pwr = (int)(-0.01*Cdrv + (0.08*mSpd + 0.12*mPos) + (0.8*gSpd + 15*gAng));
    if (pwr > 100)
    {
        pwr = 100;
    }
    if (pwr < -100)
    {
        pwr = -100;
    }
}   //EQ

/**
 * This function calculates the turn power.
 */
void cntrl(int &leftPwr, int &rightPwr)
{
#ifdef _ENABLE_DEBUG
    pwr /= 2; //Slow it down for debugging
#endif
    mPos -= Cdrv*tInt;
    int turnPwr = (int)(Cstr*0.1);
    leftPwr = pwr - turnPwr;
    rightPwr = pwr + turnPwr;
}   //cntrl

/**
 * This function checks if we crashed. We crashed if the motors are running
 * at full power for over a second.
 */
bool CHK()
{
    bool fQuit;

#ifdef _ENABLE_DEBUG
    nxtDisplayTextLine(0, "Cdrv=%3d,Cstr=%3d", Cdrv, Cstr);
    nxtDisplayTextLine(1, "pwr=%4d,gyro=%3d", pwr, SensorValue[gyroSensor]);
#endif
    if (abs(pwr) < 100)
    {
        ClearTimer(T2);
        fQuit = false;
    }
    if (time1[T2] > 1000)
    {
        fQuit = true;
    }

    return fQuit;
}   //CHK

task main()
{
    int startTime;
    int leftPwr, rightPwr;

#ifdef _USE_BHV
    StartTask(BHV);
#endif

    while (true)
    {
        RST();
        nxtDisplayRICFile(0, 0, "Sleeping.ric");
        calGyroOffset();
        gAng = -0.25;
        PlaySoundFile("Speed up.rsf");
        nxtDisplayRICFile(0, 0, "Awake.ric");
        st = 1;
        //
        // Balance loop
        //
        do
        {
            GT();
            startTime = time1[T1];
            GG();
            GM();
            EQ();
            cntrl(leftPwr, rightPwr);
            motor[leftMotor] = leftPwr;
            motor[rightMotor] = rightPwr;
            ok = CHK();
            //
            // 5ms loop
            //
            wait1Msec(5 - (time1[T1] - startTime));
        } while (!ok);

        motor[leftMotor] = 0;
        motor[rightMotor] = 0;
        st = 0;
        nxtDisplayRICFile(0, 0, "Knocked out.ric");
        PlaySoundFile("Speed down.rsf");
        //
        // Wait for touch sensor.
        //
        while (SensorValue[touchSensor] == 0)
        {
        }
    }
}   //task main


Fri Sep 06, 2013 6:35 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 1 post ] 

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.