element14 Community
element14 Community
    Register Log In
  • Site
  • Search
  • Log In Register
  • About Us
  • Community Hub
    Community Hub
    • What's New on element14
    • Feedback and Support
    • Benefits of Membership
    • Personal Blogs
    • Members Area
    • Achievement Levels
  • Learn
    Learn
    • Ask an Expert
    • eBooks
    • element14 presents
    • Learning Center
    • Tech Spotlight
    • STEM Academy
    • Webinars, Training and Events
    • Learning Groups
  • Technologies
    Technologies
    • 3D Printing
    • FPGA
    • Industrial Automation
    • Internet of Things
    • Power & Energy
    • Sensors
    • Technology Groups
  • Challenges & Projects
    Challenges & Projects
    • Design Challenges
    • element14 presents Projects
    • Project14
    • Arduino Projects
    • Raspberry Pi Projects
    • Project Groups
  • Products
    Products
    • Arduino
    • Avnet Boards Community
    • Dev Tools
    • Manufacturers
    • Multicomp Pro
    • Product Groups
    • Raspberry Pi
    • RoadTests & Reviews
  • Store
    Store
    • Visit Your Store
    • Choose another store...
      • Europe
      •  Austria (German)
      •  Belgium (Dutch, French)
      •  Bulgaria (Bulgarian)
      •  Czech Republic (Czech)
      •  Denmark (Danish)
      •  Estonia (Estonian)
      •  Finland (Finnish)
      •  France (French)
      •  Germany (German)
      •  Hungary (Hungarian)
      •  Ireland
      •  Israel
      •  Italy (Italian)
      •  Latvia (Latvian)
      •  
      •  Lithuania (Lithuanian)
      •  Netherlands (Dutch)
      •  Norway (Norwegian)
      •  Poland (Polish)
      •  Portugal (Portuguese)
      •  Romania (Romanian)
      •  Russia (Russian)
      •  Slovakia (Slovak)
      •  Slovenia (Slovenian)
      •  Spain (Spanish)
      •  Sweden (Swedish)
      •  Switzerland(German, French)
      •  Turkey (Turkish)
      •  United Kingdom
      • Asia Pacific
      •  Australia
      •  China
      •  Hong Kong
      •  India
      •  Korea (Korean)
      •  Malaysia
      •  New Zealand
      •  Philippines
      •  Singapore
      •  Taiwan
      •  Thailand (Thai)
      • Americas
      •  Brazil (Portuguese)
      •  Canada
      •  Mexico (Spanish)
      •  United States
      Can't find the country/region you're looking for? Visit our export site or find a local distributor.
  • Translate
  • Profile
  • Settings
Twist, Turn and Move Design Challenge with TE Robotics
  • Challenges & Projects
  • Design Challenges
  • Twist, Turn and Move Design Challenge with TE Robotics
  • More
  • Cancel
Twist, Turn and Move Design Challenge with TE Robotics
Blog Rullit #6 - The software (update)
  • Blog
  • Forum
  • Documents
  • Polls
  • Files
  • Leaderboard
  • Mentions
  • Sub-Groups
  • Tags
  • More
  • Cancel
  • New
  • Share
  • More
  • Cancel
Group Actions
  • Group RSS
  • More
  • Cancel
Engagement
  • Author Author: amgalbu
  • Date Created: 8 Sep 2022 8:31 AM Date Created
  • Views 874 views
  • Likes 6 likes
  • Comments 4 comments
  • rullit
  • te connectivity
  • Twist Turn and Move Robotics Design Challenge
  • terobotics
  • designchallenge
  • Turn and Move Design Challenge with TE Robotics
Related
Recommended

Rullit #6 - The software (update)

amgalbu
amgalbu
8 Sep 2022
Rullit #6 - The software (update)

After some preliminary tests, I realized the approach I followed was not to best one, so I made some correction to the Rullit’s control software

 

Problem

There is  basically one major problem in the implementation I talked about in my previous post [link]: because of the low threshold in PWM values (required to prevent motors from stalling), motors are switched on and off very rapidly. This causes many current peaks. In general, I noticed that every time the motor is started under load, there is a current peak. Since the L298P (the component the DFRobot DRI0017 is built on) is rated for current peaks up to 2.5A, I set the overcurrent protection on my power bench to this value and checked whether in any condition this value is exceeded

 

My solution

The solution I chose to implement is to change the path of the robot. I am going to keep the left motor stopped and I will drive the right motor until one of the limit switches is triggered. The resulting path is something like this

image

This solutions has two advantages

  • it does not require a precise control of the motor speed, because all the logic is controller by limit switches
  • it does require any knowledge about the size of the PV modules, because again all the logic is controller by limit switches

Depending on the limit switch that has triggered, I am taking different actions

I created a function (stepMotor) to

void stepMotor(int motor, int dir, int perc)
{
 Serial.print("stepMotor ");
 Serial.print((motor == LeftMotor)? "LEFT " : "RIGHT ");
 Serial.print((dir == WIND_DIR)? "WIND " : "UNWIND ");

 Serial.print(perc);
 Serial.print("%");
 Serial.println();

 moveMotor(motor, dir, perc);
 delay(PATH_STEP_MS);
 stopMotor(motor);

 Serial.print("stepMotor completed");
}

This function winds or unwinds left or right rope of a fixed length

 

Left limit switch

When the left limit switch is triggered, the right rope is winded to move the robot back at safety distance from the edge, then the left rope is unwinded to let Rullit clean the adjacent slice of PV module.

image

Finally, the right motor is started in order to wind the right rope and move the robot toward the top of the PV module

void handleLeftLimit()
{
 Serial.println(">>> LEFT LIMIT");
 stepMotor(RightMotor, WIND_DIR, WIND_PERC);
 stepMotor(LeftMotor, UNWIND_DIR , WIND_PERC);

 Serial.println("moveMotor RIGHT WIND");
 moveMotor(RightMotor, WIND_DIR, WIND_PERC);
}

 

Bottom limit switch

When the bottom limit switch is triggered, the left and right motor are winded to move the robot back at safety distance from the edge, then the left motor is unwinded twice to let Rullit clean the adjacent slice of PV module.

image

Finally, the right motor is started in order to wind the right rope and move the robot toward the top of the PV module

void handleBottomLimit()
{
 Serial.println(">>> BOTTOM LIMIT");
 stepMotor(LeftMotor, WIND_DIR, WIND_PERC);
 stepMotor(RightMotor, WIND_DIR, WIND_PERC);
 stepMotor(LeftMotor, UNWIND_DIR, WIND_PERC);
 stepMotor(LeftMotor, UNWIND_DIR, WIND_PERC);

 Serial.println("moveMotor RIGHT WIND");
 moveMotor(RightMotor, WIND_DIR, WIND_PERC);
}

 

Top limit switch

When the top limit switch is triggered, the left rope is unwinded and right rope is winded to move the robot back at safety distance from the edge, then the left motor is unwinded twice to let Rullit clean the adjacent slice of PV module.

image

Finally, the right motor is started in order to unwind the right rope and move the robot toward the top of the PV module

 

void handleTopLimit()
{
 Serial.println(">>> TOP LIMIT");
 stepMotor(LeftMotor, UNWIND_DIR, WIND_PERC);
 stepMotor(RightMotor, WIND_DIR, WIND_PERC);
 stepMotor(LeftMotor, UNWIND_DIR, WIND_PERC);

 Serial.println("moveMotor RIGHT UNWIND");
 moveMotor(RightMotor, UNWIND_DIR, WIND_PERC);
}

 

Right limit switch

When the right limit switch is triggered, the right rope is unwinded to move the robot back at safety distance from the edge, then the left motor is unwinded twice to let Rullit clean the adjacent slice of PV module.

image

Finally, the right motor is started in order to unwind the right rope and move the robot toward the top of the PV module

void handleRightLimit()
{
 Serial.println(">>> RIGHT LIMIT");
 stepMotor(RightMotor, UNWIND_DIR, WIND_PERC);
 stepMotor(LeftMotor, UNWIND_DIR, WIND_PERC);

 Serial.println("moveMotor RIGHT UNWIND");
 moveMotor(RightMotor, UNWIND_DIR, WIND_PERC);
}

 

 

Clean completion

The clean process is completed when right and bottom switch are triggered less than 15 seconds apart. This means we are enough closed to the bottom right corner of PV modules

 

Soft start

As a further protection against current peaks, I will implement a soft-start algorithm, where the motor are started with increasing PWM values, according to a fixed sequence. Currently, I am experimenting with this sequence

10%

20%

50%

100%

Each step in the sequence last 500 ms, so it takes about 2 seconds to go from 0 to the nominal RPMs

 

void moveMotor(int motorNumber, int dir, float perc)
{
 float sign = +1.0;

 if (motorNumber == LeftMotor) {
  if (dir == WIND_DIR)
   sign = -1.0;
 }
 else {
  if (dir == UNWIND_DIR)
   sign = -1.0;
 }

 for (int i=0; i<sizeof(SoftStartSteps)/sizeof(float); i++)
 {
  moveMotor(motorNumber, sign * SoftStartSteps[i]);
  delay(SOFTSTART_STEPS_DELAY);
 }

 moveMotor(motorNumber, sign * perc);
}

Prev: The software

Next: User interface

  • Sign in to reply
  • robogary
    robogary over 2 years ago in reply to amgalbu

    Take comfort that people who never let the smoke out arent trying hard enough. 

    • Cancel
    • Vote Up 0 Vote Down
    • Sign in to reply
    • More
    • Cancel
  • amgalbu
    amgalbu over 2 years ago in reply to robogary

    Hello robogary

    I wish I has read your comment before. I was misguided by the fact that the stalled motor with 10% PWM drew a mere 300 mA, whereas at 100% PWM the over-current protection was triggered. This lead me to think that applying stepped PWM values would solve the problem. As shown in practice I was wrong...  

    • Cancel
    • Vote Up 0 Vote Down
    • Sign in to reply
    • More
    • Cancel
  • robogary
    robogary over 2 years ago

    sorry, i forgot to mention why soft start doesnt limit instantaneous current. The PWM voltage is battery volts, always. Chopping the pwm waveform to 10% duty reduces the average volts,  reducing average current , but not the peaks. 

    • Cancel
    • Vote Up 0 Vote Down
    • Sign in to reply
    • More
    • Cancel
  • robogary
    robogary over 2 years ago

    Nicely written blog. If the failure mode is truely instantaneous current, a soft start probably wont help. The soft start doesnt limit current of the stalled motor, but it does limit the time spent in instantaneous current while stalled. A short term patch could be to simply add a series resistor with the motor. Measure the motor's resistance at the motor terminals, take several readings, and turn the shaft a bit for each reading so get an average resistance measurement. Use your battery voltage's highest value, and divide by 2 amps to get a desired circuit resistance value. Leave some margin in the peak amps rating. Subtract the measured motor resistance, the remaining resistance is your stall current protection resistance.  

    Interestingly enough, that same circuit was used in industry back in the analog control system/dc motor days. It is called a droopy speed regulator. For example, if a conveyor has alot of driven rolls for something heavy like a steel girder, if every roll is a hard speed regulator,  each will go to limit to push that girder. Each roll wants to set the girder speed, but nothing is perfect, so they will all fight each other. Using a droop resistor in the motor circuits, the roll will slow down because the resistor has more voltage drop as load increases.  All the motors were fed from really big adjustable volts dc power supply. Modern day motor controls have current feedback for every motor, so that feature can be put in software. 

    Your motor will slow down at high loads, maybe that is a desireable trait in your application. 

    • Cancel
    • Vote Up 0 Vote Down
    • Sign in to reply
    • More
    • Cancel
element14 Community

element14 is the first online community specifically for engineers. Connect with your peers and get expert answers to your questions.

  • Members
  • Learn
  • Technologies
  • Challenges & Projects
  • Products
  • Store
  • About Us
  • Feedback & Support
  • FAQs
  • Terms of Use
  • Privacy Policy
  • Legal and Copyright Notices
  • Sitemap
  • Cookies

An Avnet Company © 2025 Premier Farnell Limited. All Rights Reserved.

Premier Farnell Ltd, registered in England and Wales (no 00876412), registered office: Farnell House, Forge Lane, Leeds LS12 2NE.

ICP 备案号 10220084.

Follow element14

  • X
  • Facebook
  • linkedin
  • YouTube