ROBOTC.net forums
http://www.robotc.net/forums/

Programming the LEGO m Motor with RobotC
http://www.robotc.net/forums/viewtopic.php?f=1&t=5183
Page 1 of 1

Author:  ewilliams105 [ Wed Nov 28, 2012 4:50 pm ]
Post subject:  Programming the LEGO m Motor with RobotC

My FTC team is having difficulty programming the Lego M motor using RobotC. We are using this motor to control our claw that picks up the rings in the 2012 game. However, we only have the functionality to fully open the claw (almost to the point of breaking) and to fully close it. We cannot figure out how to get the program right in order to have more control over the claw's functionality. Anyone know of a good reference we can look at for info on how to program this special type of motor. My team seems to think it more so designed for programming in LabView, but there must be a way to control it with RobotC. Please help!

Many Thanks!

Author:  MHTS [ Wed Nov 28, 2012 7:02 pm ]
Post subject:  Re: Programming the LEGO m Motor with RobotC

The LEGO motors have built-in encoders. So assuming your claw is closed to start with and assuming you want to use the joystick to control it. Here is an example on how to do it. It's not the best code but it's simple.
Code:
#define DEADBAND_THRESHOLD 15
#define DEADBAND(x) ((abs(x) >= DEADBAND_THRESHOLD)? x: 0)
#define BOUND(x) (((x) > 100)? 100: ((x) < -100)? -100: (x))
#define JOYSTICK_POWER(x) (BOUND(DEADBAND(x)))

#define CLAW_CLOSE_ANGLE 0
#define CLAW_OPEN_ANGLE 120

task main()
{
    //Assume the claw is closed at start and the encoder value goes up when opening.
    nMotorEncoder[clawMotor] = CLAW_CLOSE_ANGLE;
    while (true)
    {
        //
        // Get joystick readings.
        //
        getJoystickSettings(joystick);

        //
        // Translate controller 2 right stick x axis value to within motor power range (-100 to 100) and with deadband.
        //
        int clawPower = JOYSTICK_POWER(joystick.joy2_x1);

        //
        // Read the current claw angle.
        //
        int clawPos = nMotorEncoder[clawMotor];

        //
        // Only allow the motor to move if we are opening and it's not fully opened
        // or we are closing but it's not fully closed.
        //
        if ((clawPower > 0) && (clawPos < CLAW_OPEN_ANGLE) ||
            (clawPower < 0) && (clawPos > CLAW_CLOSE_ANGLE))
        {
            motor[clawMotor] = clawPower;
        }
        else
        {
            motor[clawMotor] = 0;
        }
        wait1Msec(100);
    }
}

Page 1 of 1 All times are UTC - 5 hours [ DST ]
Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group
http://www.phpbb.com/