Skelebot Electronics & Software

The load on the DC motor is constantly changing as the candy is being spun through the auger. While the auger rotation speed will be constant, the weight distribution of the candy is subject to change. The DC motor’s speed can be controlled using a PID (Proportional-Integral-Derivative) controller, which is a popular feedback control algorithm used in many control applications. The PID controller continuously measures the actual speed of the motor and adjusts the motor's voltage or current input to maintain the desired speed.

Here is a basic overview of how a DC motor can be controlled using a PID controller: Measurement of the actual speed of the DC motor is taken using the built-in encoder. The difference between the actual speed and the desired speed (known as the "error") is calculated. The PID controller calculates the appropriate control signal to apply to the motor based on the error. The control signal is sent to the motor driver, which adjusts the motor's voltage or current input. The motor speed is monitored continuously, and the process repeats to maintain the desired speed. Maintaining a constant speed is necessary for a smooth rotation of the crank arm, L1, which drives the crank-rocker mechanism. Further breakdown of the motion is in the above section Mobility Analysis. The Figure below shows the DC motor speed being updated by the PID controller.

The three components of a PID controller are the proportional (P), integral (I), and derivative (D) terms. The proportional term adjusts the control signal based on the current error, the integral term considers the accumulated error over time, and the derivative term adjusts the control signal based on the rate of change of the error.

By adjusting the gains of the P, I, and D terms, the response of the system can be tuned to achieve optimal performance. The goal is to minimize the error between the actual and desired speed while avoiding overshoot or instability in the system.

A stepper motor directly drives the motion of the jaw in the robotic system. A stepper motor is a type of motor that moves in discrete steps rather than continuously rotating like a conventional DC motor. Because the jaw has a limited range of motion, this makes the stepper motor ideal for precise control of rotational motion.

The stepper motor is controlled by sending pulses of electricity to the motor windings in a specific sequence. This sequence of pulses causes the motor to rotate a precise number of steps in a particular direction. By controlling the frequency and duration of these pulses, the speed and position of the motor can be precisely controlled. The jaw is attached directly to the output shaft of the motor as the jaw is light enough to move without a coupled gearbox.

Project Code

/*
 * RMD Project Justin Lam Russell Phillips Austin Uresti
 */
#include <math.h>


#define Encoder_output_A 2 // pin2 of the Arduino
#define Encoder_output_B 3 // pin 3 of the Arduino
//stepper pins
#define IN1 10
#define IN2 11
#define IN3 12
#define IN4 13


#define WINDOW_SIZE 5
boolean Direction = true;
int Steps = 0;


int INDEX = 0;
int VALUE = 0;
int SUM = 0;
int READINGS[WINDOW_SIZE];
int AVERAGED = 0;


// Constant Definitions
int countsPerRev = 48;
double maxSpeed = 66.7; //in rev/sec (this is found experimentally, you may need to confirm the value)
double delayTime = 20; // in ms


// Pin Definitions
const int PWMoutp = 8;
const int PWMoutn = 7;
const int PWMspeedPin = 9;




// Variable Definitions
volatile signed int encoder0Pos = 0;
signed int encoderPosLast = 0;
int potPWMval = 0;
int motorPWMval = 0;
int PWM_BASE = 40;
double OPEN_L_G = 0.147;


//PID constants (Use the Ziegler Nicholas Tuning Method as a first guess)
double kp = 25;
double ki = 0.4;
double kd = 2;


// PID Variables
unsigned long currentTime, previousTime;
double elapsedTime;
double error;
double lastSpeedError;
double motorSpeed, newMotorSpeed, setPoint;
double cumError, rateError;
int Sum, Average;


void Motor_Run();
void Motor_Control();
void DC_Motor_Encoder();
double computePID(double inp);
void jaw();
void stepperHALF(int xw);
void SetDirection();


void setup()
{
  // Don't Change
  Serial.begin(115200);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(PWMoutp, OUTPUT);
  pinMode(PWMoutn, OUTPUT);
  pinMode(PWMspeedPin, OUTPUT);
  //attachInterrupt(encoder0PinA, doEncoderA, RISING); // Interrupt is fired whenever button is pressed
  //attachInterrupt(encoder1PinA, doEncoderA, FALLING);
  //attachInterrupt(encoder0PinB, doEncoderB, RISING);
  //attachInterrupt(encoder1PinB, doEncoderB, FALLING);
  attachInterrupt(digitalPinToInterrupt(Encoder_output_A),DC_Motor_Encoder,RISING);


 // Pre-set the direction of the motors
// digitalWrite(PWMoutp, HIGH);
// digitalWrite(PWMoutn, LOW);


 // USE FOR PID CONTROL
 setPoint = 2;                          //set point at zero rev/second
 //Serial.print("Motor_speed"); // CHANGE THIS TO PLOT MOTOR SPEED
// Serial.print(",");
// Serial.println("Time");
 Serial.println("Set point , Motor_speed");
 digitalWrite(PWMoutp, LOW);
 digitalWrite(PWMoutn, HIGH);
 analogWrite(PWMspeedPin,255);
}


void loop()
{
  //Motor_Run();
  Motor_Control();
  //jaw();


}


void Motor_Run(){
      digitalWrite(PWMoutp, LOW); //Motor Direction
    digitalWrite(PWMoutn, HIGH);
    analogWrite(PWMspeedPin,150);
}
void Motor_Control(){
    digitalWrite(PWMoutp, LOW); //Motor Direction
    digitalWrite(PWMoutn, HIGH);


  // Hint: you just need to convert the encoder counts to Rev/Sec!
  motorSpeed = ((encoder0Pos - encoderPosLast)/(delayTime/1000))/ countsPerRev;


 // Serial.print(millis());
 


  // HERE YOU WILL WRITE YOUR CODE TO ENABLE THE PID CONTROLLER. Use the function below!!
  /* Initially, just have the PID controller follow the pre-defined
      setPoint variable. For fun, you can later consider how to tie the PID
      controller to the potenitometer value to have an adaptively changing setPoint.*/
   analogWrite(PWMspeedPin,computePID(motorSpeed));
   //analogWrite(PWMspeedPin,120);


  SUM = SUM - READINGS[INDEX];       // Remove the oldest entry from the sum
  VALUE = motorSpeed;        // Read the next sensor value
  READINGS[INDEX] = VALUE;           // Add the newest reading to the window
  SUM = SUM + VALUE;                 // Add the newest reading to the sum
  INDEX = (INDEX+1) % WINDOW_SIZE;   // Increment the index, and wrap to 0 if it exceeds the window size


  AVERAGED = SUM / WINDOW_SIZE;      // Divide the sum of the window by the window size for the result


   Serial.print("Set_Point:");
    Serial.print(setPoint);
    Serial.print("  ,  ");
    Serial.print("Actual_Motor_Speed:");
    Serial.println(abs(AVERAGED));


  encoderPosLast = encoder0Pos;
  delay(delayTime); // This is a delay to time the control loop. In the future, this should be a non-blocking version.
}
void DC_Motor_Encoder(){
  int b = digitalRead(Encoder_output_B);
  if(b > 0){
    encoder0Pos++;
  }
  else{
    encoder0Pos--;
  }
}
//================
// PID CONTROL FUNCTION (use this!)
//================
double computePID(double inp){    
        currentTime = millis();                //get current time
        elapsedTime = (double)(currentTime - previousTime);        //compute time elapsed from previous computation
       
        error = setPoint - inp;                         // determine error
        cumError += error * elapsedTime;                // compute integral
        rateError = (error - lastSpeedError)/elapsedTime;   // compute derivative
         (cumError > 300) ? cumError = 100 : 0;
        double out = kp*error + ki*cumError + kd*rateError + setPoint/OPEN_L_G;  //PID output   +  setPoint/OPEN_L_G ;
         
        (out > 255 ) ? out = 255 : (out < 0) ? out = 0: 0;  
        //Serial.print("Output: ");
        //Serial.print("\t");
         //Serial.print(out);        
       
        lastSpeedError = error;                            //remember current error
        previousTime = currentTime;  
        //Serial.print("\t\t");                   //remember current time
      //Serial.println(error);
        return out;                                        //have function return the PID output
}


void jaw(){
    int opening = random(500);// 20 degree opening
    //int opening = 500;
  for(int i=0; i<opening; i++){ // What is 4096? You'll have to change things here to do rotational control
  stepperHALF(1);
  Serial.println(Steps);
  delayMicroseconds(1500); // Don't change this number - see if you can find it on the scope!
  }
    Direction = !Direction;
    delay(100);
  for(int i=0; i<opening; i++){ // What is 4096? You'll have to change things here to do rotational control
  stepperHALF(1);
  Serial.println(Steps);
  delayMicroseconds(1500); // Don't change this number - see if you can find it on the scope!
  }
  Direction = !Direction;
  delay(200);
}
void stepperHALF(int xw) {
  for (int x = 0; x < xw; x++) {
  switch (Steps) {
  case 0:
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
  break;


  case 1:
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
  break;


  case 2:
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
  break;


  case 3:
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  break;


  case 4:
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  break;


  case 5:
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, HIGH);
  break;


  case 6:
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  break;


  case 7:
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  break;


  default:
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
  break;
  }
  SetDirection();
  }
  }


void SetDirection() {
  if (Direction == 1) {
  Steps++;
  }
  if (Direction == 0) {
  Steps--;
  }
  if (Steps > 7) {
  Steps = 0;
  }
  if (Steps < 0) {
  Steps = 7;
  }
}