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

Strange RobotC 4.3 with EV3
http://robotc.net/forums/viewtopic.php?f=1&t=11360
Page 1 of 1

Author:  sparramc [ Sat May 30, 2015 4:53 am ]
Post subject:  Strange RobotC 4.3 with EV3

I am having some strange behavior with the following Function:

Code:
void operateClamp()
{
   clampDirection = clampDirection * -1;
   if (clampDirection == 1) {displayCenteredTextLine(5, "Closing Container Clamp");}
   else {displayCenteredTextLine(5, "Opening Container Clamp");}
   
   resetMotorEncoder(motorD);
   moveMotorTarget(motorD, 1800, clampDirection * 100);
   waitUntilMotorStop(motorD);
}


When it is placed in this code sequence, it happily works as I designed it too:

Code:
void operateClamp()
{
   clampDirection = clampDirection * -1;
   if (clampDirection == 1) {displayCenteredTextLine(5, "Closing Container Clamp");}
   else {displayCenteredTextLine(5, "Opening Container Clamp");}
   
   resetMotorEncoder(motorD);
   moveMotorTarget(motorD, 1800, clampDirection * 100);
   waitUntilMotorStop(motorD);
}


task main()
{
   while (true)
  {
    operateClamp();
  }
}


It will continue to open and close the Clamp all day....

But when the Function is added to my main code sequence below, it operates only for the first call, after that the motorD fails to run in any direction.

Code:
/*************************************************************************************
*************************************************************************************
**                                                                                                                           **
**           ==================================================================            **
**            ||   Lego Mindstorms EV3 40' Container Handling Robotic Crane   ||            **
**           ==================================================================            **
**                                                                                                                           **
**            © 2015 www.LegoMindstormsRobots.com. This work is licensed under a            **
**            Creative Commons Attribution-ShareAlike 3.0 Unported License:                    **
**            http://creativecommons.org/licenses/by-sa/3.0/                                          **
**                                                                                                                           **
**            For full project details visit: http://www.LegoMindstormsRobots.com/         **
**                                                                                                                           **
**                                                                                                                           **
*************************************************************************************
*************************************************************************************/

task stayAlive();
task ZeroEncoders();
task MonitorEncoders();

int MoveAlongXaxis(int distanceToMove);
int MoveAlongYaxis(int distanceToMove);
void HomeYaxis();
void operateClamp();
int WinchPosition(int distanceToMove);
void SetXaxis();
void SetYaxis();
void SetWinch();
void SetClamp();
void ManualSetUp();

int WinchHome = 0, WinchUp = -500, TrainHeight = -13700, TruckHeight = -14750, WinchDown = -18000;
int MoveToTruck = 0, MoveToTrack1 = 12100, MoveToTrack2 = 19600;
bool XaxisCalibrated, YaxisCalibrated, WinchCalibrated, ClampCalibrated;

short clampDirection = 1;



/**********************************************************************************
*                                                                                                                           *
*      Control the "Container Handling Robot"                                                            *
*                                                                                                                           *
**********************************************************************************/
task main()
{
   eraseDisplay();
   clearDebugStream();
   setLEDColor(ledGreen);
   startTask(stayAlive);

   displayInverseBigStringAt(28, 35, "Calibrate?");
   displayCenteredTextLine(14, "YES = Centre Button");
   displayCenteredTextLine(15, "NO = Any Button");
   sleep(1000);

   setLEDColor(ledOrangeFlash);
   waitForButtonPress();
   if (getButtonPress(buttonEnter)) {ManualSetUp();}
   setLEDColor(ledGreen);
   eraseDisplay();

   startTask(MonitorEncoders);

   while (true)
   {
      MoveAlongYaxis(MoveToTruck);
      WinchPosition(TruckHeight);
      playSound(soundShortBlip);
      operateClamp();
      WinchPosition(WinchUp);
      playSound(soundShortBlip);
      MoveAlongYaxis(MoveToTrack1);
      WinchPosition(TrainHeight);
      playSound(soundShortBlip);
      operateClamp();
      WinchPosition(WinchUp);
      playSound(soundShortBlip);
      MoveAlongYaxis(MoveToTrack2);
      WinchPosition(TrainHeight);
      playSound(soundShortBlip);
      operateClamp();
      WinchPosition(WinchUp);
      playSound(soundShortBlip);
      MoveAlongYaxis(MoveToTruck);
      WinchPosition(TrainHeight);
      playSound(soundShortBlip);
      operateClamp();
      WinchPosition(WinchUp);
      MoveAlongYaxis(MoveToTrack1);
      WinchPosition(TrainHeight);
      playSound(soundShortBlip);
      operateClamp();
      WinchPosition(WinchUp);
      MoveAlongYaxis(MoveToTrack2);
      WinchPosition(TrainHeight);
      playSound(soundShortBlip);
      operateClamp();
      WinchPosition(WinchUp);
   }


   //   stopAllTasks();
}



/**********************************************************************************
*                                                                                                                           *
*      Keep Robot Alive                                                                                             *
*                                                                                                                           *
**********************************************************************************/
task stayAlive()
{
   while(true)
   {
      displayCenteredBigTextLine(1, "Bulk Container");
      displayCenteredBigTextLine(4, "Handling Crane");
      sleep(50);
   }
}



/**********************************************************************************
*                                                                                                                           *
*      Zero ALL Motor Encoders   when 'Back Button' is Depressed                                    *
*                                                                                                                           *
**********************************************************************************/
task ZeroEncoders()
{
   while(true)
   {
      if (getButtonPress(buttonBack))
      {
         resetMotorEncoder(motorA);
         resetMotorEncoder(motorB);
         resetMotorEncoder(motorC);
         resetMotorEncoder(motorD);
      }
      sleep(50);
   }
}



/**********************************************************************************
*                                                                                                                           *
*      Monitor All Motor Encoders                                                                              *
*                                                                                                                           *
**********************************************************************************/
task MonitorEncoders()
{
   while(true)
   {
      displayCenteredTextLine(10, "X Axis Encoder = %d", getMotorEncoder(motorA));
      displayCenteredTextLine(11, "Y Axis Encoder = %d", getMotorEncoder(motorB));
      displayCenteredTextLine(12, "Winch Encoder = %d", getMotorEncoder(motorC));
      displayCenteredTextLine(13, "Clamp Encoder = %d", getMotorEncoder(motorD));
      sleep(50);
   }
}



/**********************************************************************************
*                                                                                                                           *
*      Move Crane along X Axis                                                                                    *
*                                                                                                                           *
**********************************************************************************/
int MoveAlongXaxis(int distanceToMove)
{
   distanceToMove = distanceToMove - getMotorEncoder(motorA);
   displayCenteredTextLine(8, "Distance To Move = %d", distanceToMove);
   writeDebugStreamLine("Distance To Move = %d", distanceToMove);
   writeDebugStreamLine("Motor Encoder = %d", getMotorEncoder(motorA));
   writeDebugStreamLine("");

   moveMotorTarget(motorA, distanceToMove, sgn(distanceToMove) * 100);
   waitUntilMotorStop(motorA);
   return 0;
}



/**********************************************************************************
*                                                                                                                           *
*      Move Crane along Y Axis                                                                                    *
*                                                                                                                           *
**********************************************************************************/
int MoveAlongYaxis(int distanceToMove)
{
   distanceToMove = distanceToMove - getMotorEncoder(motorB);
   displayCenteredTextLine(8, "Distance To Move = %d", distanceToMove);
   writeDebugStreamLine("Distance To Move = %d", distanceToMove);
   writeDebugStreamLine("Motor Encoder = %d", getMotorEncoder(motorB));
   writeDebugStreamLine("");

   moveMotorTarget(motorB, distanceToMove, sgn(distanceToMove) * 100);
   waitUntilMotorStop(motorB);
   return 0;
}



/**********************************************************************************
*                                                                                                                           *
*      Home Y Axis                                                                                    *
*                                                                                                                           *
**********************************************************************************/
void HomeYaxis()
{
   repeatUntil(getTouchValue(S1))
   {
      setMotorSpeed(motorB, -100);
   }
   setMotorSpeed(motorB, 0);
   resetMotorEncoder(motorB);
}



/**********************************************************************************
*                                                                                                                           *
*      Open and Close the Container Clamp                                                                  *
*                                                                                                                           *
**********************************************************************************/
void operateClamp()
{
   sleep(2000);
      clampDirection = clampDirection * -1;
      if (clampDirection == 1) {displayCenteredTextLine(5, "Closing Container Clamp");}
      else {displayCenteredTextLine(5, "Opening Container Clamp");}

      resetMotorEncoder(motorD);
      moveMotorTarget(motorD, 1800, clampDirection * 100);
      waitUntilMotorStop(motorD);
   sleep(2000);
}



/**********************************************************************************
*                                                                                                                           *
*      Raise and Lower the Container                                                                           *
*                                                                                                                           *
**********************************************************************************/
int WinchPosition(int distanceToMove)
{
   writeDebugStreamLine("Raise and Lower the Container");

   distanceToMove = -(getMotorEncoder(motorC) - distanceToMove);

   displayCenteredTextLine(8, "Distance To Move = %d", distanceToMove);
   writeDebugStreamLine("Distance To Move = %d", distanceToMove);
   writeDebugStreamLine("Motor Encoder = %d", getMotorEncoder(motorC));
   writeDebugStreamLine("");

   moveMotorTarget(motorC, distanceToMove, sgn(distanceToMove) * 100);
   waitUntilMotorStop(motorC);
   return 0;
}




/**********************************************************************************
*                                                                                                                           *
*      Calibrate the Robot's Position along the X Axis                                                *
*                                                                                                                           *
**********************************************************************************/
void SetXaxis()
{
   XaxisCalibrated = false;
   displayInverseBigStringAt(5, 35, "Control X Axis");
   displayCenteredTextLine(14, "Use the Up and Down Buttons");
   displayCenteredTextLine(15, "Centre Button to Exit!");
   resetMotorEncoder(motorA);

   while (!getButtonPress(buttonEnter))
   {
      displayCenteredTextLine(10, "X Axis Encoder = %d", getMotorEncoder(motorA));

      while (getButtonPress(buttonUp))
      {
         XaxisCalibrated = true;
         setMotorSpeed(motorA, 30);
         displayCenteredTextLine(10, "X Axis Encoder = %d", getMotorEncoder(motorA));
      }
      setMotorSpeed(motorA, 0);

      while (getButtonPress(buttonDown))
      {
         XaxisCalibrated = true;
         setMotorSpeed(motorA, -30);
         displayCenteredTextLine(10, "X Axis Encoder = %d", getMotorEncoder(motorA));
      }
      setMotorSpeed(motorA, 0);
   }
   resetMotorEncoder(motorA);
   playSound(soundDownwardTones);
   sleep(500);
}



/**********************************************************************************
*                                                                                                                           *
*      Calibrate the Robot's Position along the Y Axis                                                *
*                                                                                                                           *
**********************************************************************************/
void SetYaxis()
{
   YaxisCalibrated = false;
   displayInverseBigStringAt(5, 35, "Control Y Axis");
   displayCenteredTextLine(14, " Use Left and Right Buttons");
   displayCenteredTextLine(15, " Centre Button to Exit!");
   resetMotorEncoder(motorB);

   while (!getButtonPress(buttonEnter))
   {
      displayCenteredTextLine(10, "Y Axis Encoder = %d", getMotorEncoder(motorB));

      while (getButtonPress(buttonRight))
      {
         YaxisCalibrated = true;
         setMotorSpeed(motorB, 100);
         displayCenteredTextLine(10, "Y Axis Encoder = %d", getMotorEncoder(motorB));
      }
      setMotorSpeed(motorB, 0);

      while (getButtonPress(buttonLeft))
      {
         if (!getTouchValue(S1))
         {
            YaxisCalibrated = true;
            setMotorSpeed(motorB, -100);
            displayCenteredTextLine(10, "Y Axis Encoder = %d", getMotorEncoder(motorB));
         }
      }
      setMotorSpeed(motorB, 0);
   }
   HomeYaxis();
   playSound(soundDownwardTones);
   resetMotorEncoder(motorB);
   sleep(500);
}



/**********************************************************************************
*                                                                                                                           *
*      Calibrate the Robot's Container Grab Winch for Position Above the Ground         *
*                                                                                                                           *
**********************************************************************************/
void SetWinch()
{
   WinchCalibrated = false;
   displayInverseBigStringAt(10, 35, "Control Winch");
   displayCenteredTextLine(14, "Use the Up and Down Buttons");
   displayCenteredTextLine(15, "Centre Button to Exit!");
   displayCenteredTextLine(10, "Winch Motor's Encoder = %i", 0);
   resetMotorEncoder(motorC);

   while (!getButtonPress(buttonEnter))
   {
      displayCenteredTextLine(10, "Winch Encoder = %i", getMotorEncoder(motorC));
      while (getButtonPress(buttonUp))
      {
         WinchCalibrated = true;
         setMotorSpeed(motorC, 100);
         displayCenteredTextLine(10, "Winch Encoder = %i", getMotorEncoder(motorC));
      }
      setMotorSpeed(motorC, 0);

      while (getButtonPress(buttonDown))
      {
         WinchCalibrated = true;
         setMotorSpeed(motorC, -100);
         displayCenteredTextLine(10, "Winch Encoder = %i", getMotorEncoder(motorC));
      }
      setMotorSpeed(motorC, 0);
   }
   playSound(soundBlip);

   if (WinchCalibrated == true)
   {
      moveMotorTarget(motorC, WinchDown, 100);
      waitUntilMotorStop(motorC);
   }
   resetMotorEncoder(motorC);
   playSound(soundDownwardTones);
   sleep(500);
}



/**********************************************************************************
*                                                                                                                           *
*      Calibrate the Robot's Container Grabs Claw Position                                          *
*                                                                                                                           *
**********************************************************************************/
void SetClamp()
{
   ClampCalibrated = false;
   displayInverseBigStringAt(10, 35, "Control Clamp");
   displayCenteredTextLine(14, "Use Left and Right Buttons");
   displayCenteredTextLine(15, "Centre Button to Exit!");
   resetMotorEncoder(motorD);

   while (!getButtonPress(buttonEnter))
   {
      displayCenteredTextLine(10, "Clamp Encoder = %d", getMotorEncoder(motorD));
      while (getButtonPress(buttonLeft))
      {
         ClampCalibrated = true;
         setMotorSpeed(motorD, -100);
         displayCenteredTextLine(10, "Clamp Encoder = %d", getMotorEncoder(motorD));
      }
      setMotorSpeed(motorD, 0);

      while (getButtonPress(buttonRight))
      {
         ClampCalibrated = true;
         setMotorSpeed(motorD, 100);
         displayCenteredTextLine(10, "Clamp Motor's Encoder = %d", getMotorEncoder(motorD));
      }
      setMotorSpeed(motorD, 0);
   }
   playSound(soundDownwardTones);
   resetMotorEncoder(motorD);
   sleep(500);
}



/**********************************************************************************
*                                                                                                                           *
*      Manually Calibrate the Robot                                                                           *
*                                                                                                                           *
**********************************************************************************/
void ManualSetUp()
{
   eraseDisplay();
   startTask(ZeroEncoders);
   playSound(soundUpwardTones);
   sleep(500);

   setBlockBackButton(true); // Disable the Back/Stop Button
   displayCenteredTextLine(8, "Enter Manual Calibration Mode");
   setLEDColor(ledOrangePulse);

   SetClamp();
   SetWinch();
   SetYaxis();
   SetXaxis();

   setLEDColor(ledGreen);
   setBlockBackButton(false); // enable the Back/Stop Button
   stopTask(ZeroEncoders);
   eraseDisplay();
   displayInverseBigStringAt(20, 45, "            ");
   displayInverseBigStringAt(20, 55, "The Robot is");
   displayInverseBigStringAt(20, 35, "Calibrated! ");
   sleep(2000);
}


It doesn't matter if the Control Loop in the "task main()" is repeated several times, the motorD never moves again.

The code isn't "Rocket Science", but after several hours of trying to get too the bottom of this issue I have had no luck. I have tried all the usual ways of running the motorD for 1800 Encoder counts, as well as just running the motor for a preset time period then turning it off. Every idea I have attempted will work on its own, but fails when I attempt to integrate it into my main program code.

If someone can please spare the time to check out my code for an answer it will be very much appreciated?

To run the code, you need only 4x Motors connected to the EV3, provided you don't use the "Calibrate Feature".

Author:  mightor [ Sat May 30, 2015 6:05 am ]
Post subject:  Re: Strange RobotC 4.3 with EV3

I just tested this here and it works fine. I used an internal build that we're going to be releasing very shortly. motorD continued to work just fine. Btw, is there a reason you're resetting the encoders before doing a move to target command?

= Xander

Author:  sparramc [ Sat May 30, 2015 9:42 am ]
Post subject:  Re: Strange RobotC 4.3 with EV3

Thanx Xander,

The encoder reset was just left over from trying to find a solution.

I have removed now, but still same issue.

Author:  sparramc [ Sat May 30, 2015 10:24 am ]
Post subject:  Re: Strange RobotC 4.3 with EV3

I have just swapped EV3 Bricks with no change. I then reinstalled 1.06x Kernel and v4.30 Firmware. But still no luck...

I have now swapped motorD and motorC with one-another, but no change.

If nothing else it is frustrating! I was hoping to have the Robot finished today, and boxed up ready to ship it down south to Hobbit Toown (Hobart) for next weekends Brixhibition event.

Oh... the joys of Robotics and the promises one does make....

Author:  sparramc [ Sun May 31, 2015 5:18 am ]
Post subject:  Re: Strange RobotC 4.3 with EV3

Finally I have solved the issue by exchange the original function with TWO New ones.

Old Function:
Code:
/**********************************************************************************
*                                                                                                                           *
*      Open and Close the Container Clamp                                                                  *
*                                                                                                                           *
**********************************************************************************/
void operateClamp()
{
   sleep(2000);
      clampDirection = clampDirection * -1;
      if (clampDirection == 1) {displayCenteredTextLine(5, "Closing Container Clamp");}
      else {displayCenteredTextLine(5, "Opening Container Clamp");}
      moveMotorTarget(motorC, 1800, clampDirection * 100);
      waitUntilMotorStop(motorC);
   sleep(2000);
}


New Function:
Code:
/**********************************************************************************
*                                                                                                                           *
*      Close the Container Clamp                                                                  *
*                                                                                                                           *
**********************************************************************************/
void CloseClamp()
{
   sleep(2000);
   displayCenteredTextLine(5, "Closing Container Clamp");
   setMotorSpeed(motorD, -100);
   sleep(2000);
   setMotorSpeed(motorD, 0);
   sleep(2000);
}

New Function:
Code:
/**********************************************************************************
*                                                                                                                        *
*      Open the Container  clamp                                                         *
*                                                                                                                           *
**********************************************************************************/
void OpenClamp()
{
   sleep(2000);
   displayCenteredTextLine(5, "Opening Container Clamp");
   setMotorSpeed(motorD, 100);
   sleep(2000);
   setMotorSpeed(motorD, 0);
   sleep(2000);
}


I look forward to seeing if I can get the Original Code running in the next release of RobotC.

Author:  mightor [ Sun May 31, 2015 6:22 am ]
Post subject:  Re: Strange RobotC 4.3 with EV3

Was there a reason you used a non-boolean type for the clampDirection variable in your first version?

= Xander

Author:  sparramc [ Mon Jun 01, 2015 2:55 am ]
Post subject:  Re: Strange RobotC 4.3 with EV3

I did try it as Boolean Function initially with no luck, but haven't been back to test it after I finally had the two new functions working.

I will experiment further time permitting as I'm under the pump organising this weekends Brixhibition Event. I was caught with my guard down, and I was made Vise-president of the Tasmanian Brick Enthusiasts Group a few months ago. Apparently I was considered a wasted talent, or some other..................

Author:  mightor [ Mon Jun 01, 2015 3:24 am ]
Post subject:  Re: Strange RobotC 4.3 with EV3

Just an FYI, using shorts is pointless on a 32 bit platform, they take up the same amount of space. Also, an int == long on the EV3 :)

= Xander

Author:  sparramc [ Mon Jun 01, 2015 3:43 am ]
Post subject:  Re: Strange RobotC 4.3 with EV3

It's more habit than anything else when it comes to short, int and long. Since the RCX days with RobotC I have just use 'int' as it was the only choice in the beginning. Plus with most of my code, storage isn't and ram tend not to be an issue.

That said, does that apply across the board? If I begin too seriously start use EV3dev with C++ or C# will that still hold true?

Author:  mightor [ Mon Jun 01, 2015 5:59 am ]
Post subject:  Re: Strange RobotC 4.3 with EV3

That would depend on the compiler, so I am not 100% sure.

= Xander

Author:  sparramc [ Mon Jun 01, 2015 6:42 am ]
Post subject:  Re: Strange RobotC 4.3 with EV3

Thanx Mate...

I'm waiting on another Container and a couple of Carriages to arrive tomorrow, then I can fully Test the Container Handling Crane out.

Have tacked an Arduino Uno on the side for lighting control.

In coming weeks I will get an sBrick (Bluetooth PF Controller) for a Train than I can begin full intergration into Anthony and my LEGO Train Layout....

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