GPSBots: Linux Tutorial: Motor protection with speedup/slowdown control

Linux Tutorial: Motor protection with speedup/slowdown control

Motor protection with speedup/slowdown control

A motor cannot be instantly switched from stopped to 100% or from full reverse to full forwards without potentially causing damage to the gears and other components. The circuitry can easily do it, and an erroneous signal could accidentally cause even the best coded algorithm to accidentally go too far. This class solves that problem by moving motor control to a timed subsystem.

Please see the IO Class Tutorial for the oIO object code.

To compile: g++ wheels.cpp io.cpp

wheels.h

#ifndef WHEELS_H
#define WHEELS_H
#include "io.h"

#define MOTOR_D 8      // the left motor's pololu #
#define MAX_CHANGE 10      // max wheel change per Update() cycle (percent * 100)
#define FULL_SPEED 127      // actual value to send to the motor to go full speed (- is full backwards)
#define MIN_SPEED 25      // minimum stall speed
#define FORWARD 0      // forward polarity (depends on motor wiring)
#define BACKWARD 1      // opposite of FORWARD
#define UPDATE_RATE 1      // update speed in Hz

class Wheels {
     public:
          Wheels(IO *oIO_ptr);      // Constructor
          void SetDesiredThrottle(double percent);      // to set throttle motor to 40%
          double GetDesiredThrottle();      // to get the current desired motor speed
          double GetCurrentThrottle();      // to get the actual motor speed
          void Update();      // resolves differences between Desired and Current
     private:
          IO *oIO;      // pointer to the IO object
          bool SetMotor(int motor_num, double percent);      // convert percent to a number (+-127)
          double cur_throttle;
          double des_throttle;
};
#endif

wheels.cpp

using namespace std;
#include <iostream>
#include <math.h>
#include <time.h>
#include <stdio.h>
#include <sys/time.h> // timers
#include <signal.h> // timers
#include "wheels.h"
#include "io.h"

void timer_handler(int x);
     IO oIO;
     Wheels oWheels(&oIO);

int main() {
     oWheels.SetDesiredThrottle(40);
     cout << "Desired Throttle = " << oWheels.GetDesiredThrottle() << endl;
     cout << "Current Throttle = " << oWheels.GetCurrentThrottle() << endl;
     struct itimerval timer1;
     signal(SIGALRM, timer_handler);
     timer1.it_interval.tv_sec = 0;      // reset val
     timer1.it_interval.tv_usec = (1/UPDATE_RATE)*1000000;      // reset val
     timer1.it_value.tv_sec = 0;      // initial val \_ 0 = stopped
     timer1.it_value.tv_usec = (1/UPDATE_RATE)*1000000;      // initial val
     setitimer(ITIMER_REAL, &timer1, NULL);
}
void timer_handler(int x) {
     signal(SIGALRM, timer_handler);      // reset, so it timers more than once
     oWheels.Update();
}

Wheels::Wheels(IO *oIO_ptr) {
oIO = oIO_ptr;
}

void Wheels::SetDesiredThrottle(double percent) {
     if (percent < -100) { percent = -100; }      // overflow protections
     if (percent > 100) { percent = 100; }

     des_throttle = percent;
}

double Wheels::GetDesiredThrottle() { return des_throttle; }
double Wheels::GetCurrentThrottle() { return cur_throttle; }

void Wheels::Update() {
          // :: actually changes the wheel speed ::
          // call every 100ms or so
          // we use this so that we don't go from full speed to stop (or full reverse) too quickly
     double diff = 0;
     double new_pct = 0;

     if (cur_throttle != des_throttle) {
          diff = des_throttle - cur_throttle;
          if (abs((int)diff) > MAX_CHANGE) {
               if (diff > 0) {
                    new_pct = cur_throttle + MAX_CHANGE;      // increase forwards (or decrease backwards)
               } else {
                    new_pct = cur_throttle - MAX_CHANGE;      // increase backwards (or decrease forwards)
               }
          } else {
               new_pct = cur_throttle + diff;      // diff is already signed
          }
          cur_throttle = new_pct;

               // send to IO port
          SetMotor(MOTOR_D, new_pct);
     }
}

bool Wheels::SetMotor(int num, double percent) {
          // :: Convert to an actual value, then send down the serial line the motor control codes ::
     if (percent < -100 || percent > 100) {
          cout << "IO::SetMotor: percent is out of range" << endl;
          return false;
     } else {
          int direction = FORWARD;
          double new_speed = ((FULL_SPEED - MIN_SPEED) * (percent/100.0));
          if (percent < 0) {
               direction = BACKWARD;
          }
          unsigned char out[4];
          out[0] = 0x80;      // start byte
          out[1] = 0x00;      // deviceid (0=motor)
          out[2] = (num*2)+direction;      // msb(7) = motor num, lsb = direction
     //cout << "percent=" << percent << "; dir=" << direction << "; out2=" << (int)out[2] << ";" << endl;
          out[3] = (int)abs((int)new_speed);      // max 0x7f
          oIO->SendSerial(out, 4);
     }
     return true;
}