ClearPath - Teknic servos

taskman

Well-known member
Hi

I am looking at using the ClearPath - Teknic servos on a project. At the moment I use a Sangiuolo to run our steppers. I would like to use a Teensy to run the servos because of the higher CPU clock speed.

I downloaded Teensyduino 1.41 and installed it in Arduino 1.8.5
I installed the ClearPath library. Here is a link to the zip file
https://www.dropbox.com/s/f86fn3bscr6c6k5/ClearPathStepGen.zip?dl=0

Here is a link to their github
https://github.com/Teknic-ClearPath


When I compile I get the following error with Teensyduino
I get the same for all the Teensy boards except Teensy++ 2.0. Teensy 2.0 has even more failures
C:\Users\xxx\OneDrive\Documents\Arduino\libraries\ClearPathStepGen\ClearPathStepGen.cpp:55:4: error: expected constructor, destructor, or type conversion before '(' token

ISR(TIMER2_COMPA_vect)

^

Error compiling for board Teensy 3.6.






When I compile it with Arduino 1.8.5 for Arduino Uno WIFI that has not been patched with Teensyduino it doesn't give any errors
Build options changed, rebuilding all
Archiving built core (caching) in: C:\Users\xxx\AppData\Local\Temp\arduino_cache_734912\core\core_arduino_avr_unowifi_e8a08f31ef0f32d5a2a2bcdf8022c9b6.a
Sketch uses 5024 bytes (15%) of program storage space. Maximum is 32256 bytes.
Global variables use 333 bytes (16%) of dynamic memory, leaving 1715 bytes for local variables. Maximum is 2048 bytes.
 
Teensy 3.x are ARM chips which work very different from AVR. This library is not compatible and needs adjustments to work with Teensy 3.x
 
Hi

I decided to go to the full Teknic setup so that I have fewer variables to worry about.

Controller, power supply and cables. It does mean the servo upgrade will have to cost a bit but I will figure out how to add it into the current pricing of the machines. I will see how I can use my 12V power supplies vs their 75V on our cheaper machines maybe.

I can now replace the following on my machine
3 controllers (master controller, wiper, accessories)
12 nuts and bolts for the controllers
5 fans and 20 nuts and bolts for fans
3 Controller covers and 12 nuts and bolts
H bridge and 4 nuts and bolts

It is worth it for me to pay $100 for the single controller because 3 x Sanguinololu or 3 x Teensy or 3 x Arduino Megas will cost me about the same + a cost of $0.5 per minute that assembly costs me + powder coated steel plates to cover the controllers.

You can see the prints here.
https://www.instagram.com/reel/CHcosysHPcr/?igshid=lepozlu7qqdc

Everything from the Hulkbuster suit and on was printed with Clearpath at 25 micron Z. Very accurate. No need to activate Z axis compensation like I have to do with a stepper. I am within 50 micron Z and 25 micron XY of the STL.

I printed a frame for glasses for a customer in 36 minutes vs her machine at 14 hours. No missed steps. No overheating.

I am looking at changing all my machines to Teknic. Less assembly time, works first time, no messing around.
 
The video looks very interesting. Good job !

Am i right to assume that Paul's teensy version of the original ClearPath Arduino Step+Dir library was used in your setup?
Just wondering how easy it was to move from Arduino to Teensy.
I usually use the teensy 3.2 for my projects and would hope to also do it for the ClearPath SD project.

Thank you.
Stephan.
 
Thank you :)

No I used all the Teknic components. So their controller
https://www.teknic.com/products/io-motion-controller/

I had to rewrite the whole controller code. At the moment I only needed Z so it wasn't too difficult.

I think you can get a 30 day return if you use Clearcore to see if you can convert the code. Their pins are completely different and also the way to activate the servo is completely different than a stepper.
 
I am using the ClearPath SD motor (step direction) and had it working well with an Arduino Uno and this Arduino Library

I now moved over to the teensy3.2 and had to make a few changes to get the motor moving.
A:
comment out all digitalWriteFast call but pin 11 (step pin)
https://github.com/PaulStoffregen/C...no/ClearPathStepGen/ClearPathStepGen.cpp#L124
B:
change `myTimer.begin(isr, (float)(time + 1) * 2e-6);` here: https://github.com/PaulStoffregen/C...no/ClearPathStepGen/ClearPathStepGen.cpp#L356
I think (float)(time + 1) * 2e-6 translates to 0.0005micro = 2kHz ?
But this seems way too fast.
Anything lower than 700 prevents the library from functioning. `myTimer.begin(isr,800);`

Any idea why?
Thanks for any advice.
 
The ClearPath-MSP monitor app does report "step input timing error".

I found these points here:

A step input timing error in a ClearPath motor is caused when the motor detects electrical noise on the step input. Generally this error is caused by one of the following:

-The controller is outputting less than 4VDC and is unable to reliably turn on the ClearPath motor's optically isolated input.

-The controller uses open collector outputs and the resistor specified in the ClearPath manual (link) is not installed correctly. Generally this will only affect machines with cable runs longer than 10ft, however, in applications with a large amount of electrical noise (such as plasma cutting applications), the resistors may be needed on shorter cable runs. The Acorn uses 24VDC open collector outputs, so 10Kohm resistors across the Step outputs and Direction outputs would be appropriate.

-The Controller's maximum step pulse frequency is higher than 700kHz or the minimum step pulse time is less than 715 nS (link). These settings are typically configurable in the controller.


- My cable is about 10 feet, so pretty long, but it did work with an Arduino Uno and slower intervalTimer intervals.
- maybe the direction pin (which is not set inside the ISR) and the step pin (which IS set inside the isr() ) are out of sync at higher HZ?
- I am taking the 5V to my SN74ABT125N from the teensy Vin, which comes from my computer. Maybe that's too low for higher HZ?
 
Removing this cil() and the related sei()
https://github.com/PaulStoffregen/C...Uno/ClearPathStepGen/ClearPathStepGen.cpp#L65

But reading about the purpose of cil() and sei() here makes we worried about not using them. Especially since I am still planning to add RS484 serial communication to the firmware.

FYI:
Code:
myTimer.begin(isr,200);
works and moves the motor much faster than using the Arduino Uno.
Code:
myTimer.begin(isr,10);
causes a "Tracking error limit" as shown by the ClearPath-MSP monitor app.
 
Hi everyone,
I have teensy 3.6 with 5v extension board that I designed and it works with teensystep very well. I add the schematic file of extension board, so you can examine if you want. The code that I used works with UNO but it could not work with Teensy. I tried Paul's revised code but motor did not move. Pins that I used for step signal(INPUT B for clearpath) are 2, 4, 6, 8, 29 and 38 at the Teensy side. Also change pin numbers in ClearPathStepGen.cpp But nıone of them works. Then I connect the step pin to oscilloscope, could not read any signals. Do you have any opinion? Thanks in advance
---
Paul's code that changed pin numbers
Code:
/*
  ClearPathStepGen.h - Interrupt driven library for Sending High frequency Step signals to Clearpath motors using an Arduino- Version 1
  Teknic 2017 Brendan Flosenzier

  This library is free software; you can redistribute it and/or
  modify it.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
*/

/* 
  
 A ClearPathStepGen is activated by creating an instance of the ClearPathStepGen class, and passing the constructor references to the motors it will control.
  The motors are pulsed in the background according to move information stored within each motor.
  Only motors of type; ClearPathMotorSD may be used.

  There can only be one instance of Step Controller at any time.

  This class uses Timer2, so other functions and classes which use timer 2 will not work correctly ie: tone(), MsTimer2() etc.

  The ISR is set to 2KHz, nominally

  Note: Each attached motor must have its direction/B pin connected to one of pins 8-13

  other devices can be connected to pins 8-13 as well

 
   Start(time)     - gets Direction pins for all connected motors (make sure all motors have been attached before this is called
						Configures the ISR to run at 2kHz
   Stop() - disables the ISR in this class
   
 */
#include "Arduino.h"
#include "ClearPathMotorSD.h"
#include "ClearPathStepGen.h"


// Declare Variables used in this class
// They aren't private because the ISR needs to access them
ClearPathMotorSD* _motors[6];					//6 clearpath motor pointers for up to 6 digital pins in PORTB
uint8_t _numAxis=0;							//this keeps track of how many pointers are active
uint8_t _BurstSteps[6]={0, 0, 0, 0, 0, 0};	//this is the container for the motors to dump however many steps need to be pulsed
uint8_t _pins[6]={0, 0, 0, 0, 0, 0};		//This holds the port address (Binary) for each motors Step Pin
uint8_t _SUMPINS=0;							//This holds the Binary Sum of all active motor Step Pin addresses
uint8_t _OutputBits;						//this is the container to write to output PORTB
boolean _flag=false;						//This is the flag to show when to finish pulsing the motors




//This is the Interupt Service Routine.
// It asks each motor how many steps to send, and then pulses to PORTB
#if defined(__AVR__)
ISR(TIMER2_COMPA_vect)
#elif defined(__arm__) && defined(TEENSYDUINO)
// discussion about this port for 32 bit Teensy...
// https://forum.pjrc.com/threads/51236-ClearPath-Teknic-servos
static IntervalTimer myTimer;
static void isr()
#endif
{  
	//Prevent Interupts
	cli();

//Turn on pin 2 to see how long the ISR takes
//  digitalWrite(2,HIGH);

//Poll all axes to fill BurstSteps[]
  for(int i=0;i<_numAxis;i++)
  {
	  _BurstSteps[i]=_motors[i]->calcSteps();
  }
	  

	  
//loop through BurstSteps decrementing each value to 0	  
do
{

	 _flag=false;

#if defined(__AVR__)
 _OutputBits = PORTB;	//Read the port
#elif defined(__arm__) && defined(TEENSYDUINO)
 // this is redundant, right?
#endif

if(_BurstSteps[0] && _BurstSteps[0]--)	//Assume at least one axis is active, and check/decrement BurstSteps
{
		_flag=true;
_OutputBits |= _pins[0];	//Activate the B input for motor 1
}
if(_pins[1] > 0 && _BurstSteps[1] && _BurstSteps[1]--)	//Check if Axis is active, then check/decrement BurstSteps
{
		_flag=true;
_OutputBits |= _pins[1];	//Activate the B input for motor 2
}
if(_pins[2] > 0 && _BurstSteps[2] && _BurstSteps[2]--)	//Check if Axis is active, then check/decrement BurstSteps
{
		_flag=true;
_OutputBits |= _pins[2];	//Activate the B input for motor 3
}
if(_pins[3] > 0 && _BurstSteps[3] && _BurstSteps[3]--)	//Check if Axis is active, then check/decrement BurstSteps
{
		_flag=true;
_OutputBits |= _pins[3];	//Activate the B input for motor 4
}
if(_pins[4] > 0 && _BurstSteps[4] && _BurstSteps[4]--)	//Check if Axis is active, then check/decrement BurstSteps
{
		_flag=true;
_OutputBits |= _pins[4];	//Activate the B input for motor 5
}
if(_pins[5] > 0 && _BurstSteps[5] && _BurstSteps[5]--)	//Check if Axis is active, then check/decrement BurstSteps
{
		_flag=true; 
_OutputBits |= _pins[5];	//Activate the B input for motor 6
}

#if defined(__AVR__)
PORTB = _OutputBits;			//Write to the ports
#elif defined(__arm__) && defined(TEENSYDUINO)
  digitalWriteFast(2, (_OutputBits & 1) ? HIGH : LOW);
  digitalWriteFast(4, (_OutputBits & 2) ? HIGH : LOW);
  digitalWriteFast(6, (_OutputBits & 4) ? HIGH : LOW);
  digitalWriteFast(8, (_OutputBits & 8) ? HIGH : LOW);
  digitalWriteFast(29, (_OutputBits & 16) ? HIGH : LOW);
  digitalWriteFast(38, (_OutputBits & 32) ? HIGH : LOW);
#endif

  delayMicroseconds(2);			//Short Delay
	_OutputBits &=63-_SUMPINS ;	//Turn off all active pins

#if defined(__AVR__)
  PORTB = _OutputBits;			//Write to the ports
#elif defined(__arm__) && defined(TEENSYDUINO)
  digitalWriteFast(2, (_OutputBits & 1) ? HIGH : LOW);
  digitalWriteFast(4, (_OutputBits & 2) ? HIGH : LOW);
  digitalWriteFast(6, (_OutputBits & 4) ? HIGH : LOW);
  digitalWriteFast(8, (_OutputBits & 8) ? HIGH : LOW);
  digitalWriteFast(29, (_OutputBits & 16) ? HIGH : LOW);
  digitalWriteFast(38, (_OutputBits & 32) ? HIGH : LOW);
  // TODO: is a delay needed here for processors much faster than 8 bit AVR ???
#endif
  
} while(_flag);

//turn off debug pin
//digitalWrite(2,LOW);
//allow interupts
sei();

}

/* This is the minimum constructor for ClearPathStepGen it requires a pointer to one ClearPathMotorSD, or
	one ClearPathMotorSD motor.  It requires a pointer because this class must use the functions of the
	same clearpath object used in the main routine.

	This function saves the clearpath pointer, and establishes the pins of said motor
	NOTE: the DirPins of the passed motor must be one of pins 8-13
*/
ClearPathStepGen::ClearPathStepGen(ClearPathMotorSD* motor1)
{
	_SUMPINS=0;
	_flag=false;
   _numAxis=1;
   _motors[0]=motor1;
   if(_motors[0]->PinB-8 >= 0)
	_pins[0]=(1<<(_motors[0]->PinB-8));
   _SUMPINS=_pins[0];
  
}

/* This is a constructor for ClearPathStepGen, it requires 2 pointer to ClearPathMotorSD, or
	ClearPathMotorSD motors.  It requires pointers because this class must use the functions of the
	same clearpath objects used in the main routine.

	This function saves the clearpath pointers, and establishes the pins of said motors
	NOTE: the DirPins of the passed motor must each be one different of pins 8-13
*/
ClearPathStepGen::ClearPathStepGen(ClearPathMotorSD* motor1, ClearPathMotorSD* motor2)
{
	_SUMPINS=0;
	_flag=false;
   _numAxis=2;
   _motors[0]=motor1;
   _motors[1]=motor2;
   if(_motors[0]->PinB-8 >= 0)
	_pins[0]=(1<<(_motors[0]->PinB-8));
   if(_motors[1]->PinB-8 >= 0)
    _pins[1]=(1<<(_motors[1]->PinB-8));
   _SUMPINS=_pins[0]+_pins[1];
  
}

/* This is a constructor for ClearPathStepGen, it requires 3 pointer to ClearPathMotorSD, or
	ClearPathMotorSD motors.  It requires pointers because this class must use the functions of the
	same clearpath objects used in the main routine.

	This function saves the clearpath pointers, and establishes the pins of said motors
	NOTE: the DirPins of the passed motor must each be one different of pins 8-13
*/
ClearPathStepGen::ClearPathStepGen(ClearPathMotorSD* motor1, ClearPathMotorSD* motor2, ClearPathMotorSD* motor3)
{
	_SUMPINS=0;
	_flag=false;
   _numAxis=3;
   _motors[0]=motor1;
   _motors[1]=motor2;
   _motors[2]=motor3;
   if(_motors[0]->PinB-8 >= 0)
	_pins[0]=(1<<(_motors[0]->PinB-8));
   if(_motors[1]->PinB-8 >= 0)
    _pins[1]=(1<<(_motors[1]->PinB-8));
   if(_motors[2]->PinB-8 >= 0)
    _pins[2]=(1<<(_motors[2]->PinB-8));
   _SUMPINS=_pins[0]+_pins[1]+_pins[2];
  
}

/* This is a constructor for ClearPathStepGen, it requires 4 pointer to ClearPathMotorSD, or
	ClearPathMotorSD motors.  It requires pointers because this class must use the functions of the
	same clearpath objects used in the main routine.

	This function saves the clearpath pointers, and establishes the pins of said motors
	NOTE: the DirPins of the passed motor must each be one different of pins 8-13
*/
ClearPathStepGen::ClearPathStepGen(ClearPathMotorSD* motor1, ClearPathMotorSD* motor2, ClearPathMotorSD* motor3, ClearPathMotorSD* motor4)
{
	_SUMPINS=0;
	_flag=false;
   _numAxis=4;
   _motors[0]=motor1;
   _motors[1]=motor2;
   _motors[2]=motor3;
   _motors[3]=motor4;
   if(_motors[0]->PinB-8 >= 0)
	_pins[0]=(1<<(_motors[0]->PinB-8));
   if(_motors[1]->PinB-8 >= 0)
    _pins[1]=(1<<(_motors[1]->PinB-8));
   if(_motors[2]->PinB-8 >= 0)
    _pins[2]=(1<<(_motors[2]->PinB-8));
   if(_motors[3]->PinB-8 >= 0)
   _pins[3]=(1<<(_motors[3]->PinB-8));
   _SUMPINS=_pins[0]+_pins[1]+_pins[2]+_pins[3];
  
}

/* This is a constructor for ClearPathStepGen, it requires 5 pointer to ClearPathMotorSD, or
	ClearPathMotorSD motors.  It requires pointers because this class must use the functions of the
	same clearpath objects used in the main routine.

	This function saves the clearpath pointers, and establishes the pins of said motors
	NOTE: the DirPins of the passed motor must each be one different of pins 8-13
*/
ClearPathStepGen::ClearPathStepGen(ClearPathMotorSD* motor1, ClearPathMotorSD* motor2, ClearPathMotorSD* motor3, ClearPathMotorSD* motor4, ClearPathMotorSD* motor5)
{
	_SUMPINS=0;
	_flag=false;
   _numAxis=5;
   _motors[0]=motor1;
   _motors[1]=motor2;
   _motors[2]=motor3;
   _motors[3]=motor4;
   _motors[4]=motor5;
   if(_motors[0]->PinB-8 >= 0)
	_pins[0]=(1<<(_motors[0]->PinB-8));
   if(_motors[1]->PinB-8 >= 0)
    _pins[1]=(1<<(_motors[1]->PinB-8));
   if(_motors[2]->PinB-8 >= 0)
    _pins[2]=(1<<(_motors[2]->PinB-8));
   if(_motors[3]->PinB-8 >= 0)
    _pins[3]=(1<<(_motors[3]->PinB-8));
   if(_motors[4]->PinB-8 >= 0)
    _pins[4]=(1<<(_motors[4]->PinB-8));
   _SUMPINS=_pins[0]+_pins[1]+_pins[2]+_pins[3]+_pins[4];
  
}

/* This is a constructor for ClearPathStepGen, it requires 6 pointer to ClearPathMotorSD, or
	ClearPathMotorSD motors.  It requires pointers because this class must use the functions of the
	same clearpath objects used in the main routine.

	This function saves the clearpath pointers, and establishes the pins of said motors
	NOTE: the DirPins of the passed motor must each be one different of pins 8-13
*/
ClearPathStepGen::ClearPathStepGen(ClearPathMotorSD* motor1, ClearPathMotorSD* motor2, ClearPathMotorSD* motor3, ClearPathMotorSD* motor4, ClearPathMotorSD* motor5, ClearPathMotorSD* motor6)
{
	_SUMPINS=0;
	_flag=false;  
   _numAxis=6;
   _motors[0]=motor1;
   _motors[1]=motor2;
   _motors[2]=motor3;
   _motors[3]=motor4;
   _motors[4]=motor5;
   _motors[5]=motor6;
   if(_motors[0]->PinB-8 >= 0)
	_pins[0]=(1<<(_motors[0]->PinB-8));
   if(_motors[1]->PinB-8 >= 0)
    _pins[1]=(1<<(_motors[1]->PinB-8));
   if(_motors[2]->PinB-8 >= 0)
    _pins[2]=(1<<(_motors[2]->PinB-8));
   if(_motors[3]->PinB-8 >= 0)
    _pins[3]=(1<<(_motors[3]->PinB-8));
   if(_motors[4]->PinB-8 >= 0)
    _pins[4]=(1<<(_motors[4]->PinB-8));
   if(_motors[5]->PinB-8 >= 0)
    _pins[5]=(1<<(_motors[5]->PinB-8));
   _SUMPINS=_pins[0]+_pins[1]+_pins[2]+_pins[3]+_pins[4]+_pins[5];
  
}

/*	
	This function sets up the ISR to run at 2kHz if it is passed the value of 249.
	It also, rechecks the direction pins of each connected motor
*/
void ClearPathStepGen::Start()
{
	int time = 249;   //Set Frequency 2kHz
	_SUMPINS=0;
	for( int i=0; i<_numAxis; i++)
   {
	   if(_motors[i]->PinB-8 >= 0)
			_pins[i]=(1<<(_motors[i]->PinB-8));
	   _SUMPINS+=_pins[i];
   }
	
#if defined(__AVR__)
	cli();//stop interrupts

   // set up Timer 1
   TCCR2A = 0;// set entire TCCR2A register to 0
  TCCR2B = 0;// same for TCCR2B
  TCNT2  = 0;//initialize counter value to 0

  //Set compare match register to time
  OCR2A = time;// time should be 249, 1-256 will produce different frequencies

  // turn on CTC mode
  TCCR2A |= (1 << WGM21);

  // Set CS21 bit for 8 prescaler
  TCCR2B = 0;
  TCCR2B |= (1 << CS01) | (1 << CS00);  

  // enable timer compare interrupt
  TIMSK2=0;
  TIMSK2 |= (1 << OCIE2A);

  sei();//allow interrupts

#elif defined(__arm__) && defined(TEENSYDUINO)

  myTimer.begin(isr, (float)(time + 1) * 2e-6);

#endif
}

/*	
	This function disables the ISR so any motor connected will no longer output pulses
	The motors should remember what command they were on, but if a move was interuppted
	by this function, large accelerations/deccelerations may be experienced.
*/
void ClearPathStepGen::Stop()
{
#if defined(__AVR__)
	cli();//stop interrupts
//   
   // set up Timer 1
   TCCR2A = 0;// set entire TCCR2A register to 0
  TCCR2B = 0;// same for TCCR2B
  TCNT2  = 0;//initialize counter value to 0

  sei();//allow interrupts

#elif defined(__arm__) && defined(TEENSYDUINO)

  myTimer.end();

#endif
}

// This is a debugging function
int ClearPathStepGen::getsum()
{
	return _motors[0]->calcSteps();
}

----
The arduino code that I wrote
Code:
// This code is written by EASA ALIABBASI (ealiabbasi20@ku.edu.tr) to motorize a part of a fiber winding machine.
// It reads the output values of a tension sensor and calculates the exact and real tension values
// then, applies a PID controller to control a Clear Path Tecknic stepper motor and hold the tension on a desired value.
// It is also written in a way to maintain the security of the system. The motor will stop when the rail wagon gets very close to the ends.
// Please, move the motor so that the rail wagon stays approximately at the middle. Then, run the code!
// Do not forget to change the desired tenion value!
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//Include necessary libraries
#include <ClearPathMotorSD.h>
#include <ClearPathStepGen.h>

//PID constants
double kp = 35;
double ki = 0;
double kd = 0;
double SetPoint ; // will be the desired value

unsigned long currentTime, previousTime;
double elapsedTime;
double error;
double lastError;
double input, output, setPoint;
double cumError, rateError;

// Initialization
float sensorVal_cal = 0;
boolean HLFB_Val = 0;
int Steps;
long Position;

// initialize ClearPathMotorSD Motor
ClearPathMotorSD X;
//initialize the controller and pass the reference to the motor we are controlling
ClearPathStepGen machine(&X);

void setup(){
        //Hardcode the tension value
        SetPoint = 2.08;
        //Begin Serial Communication // NOTE: If communication lags, consider increasing baud rate
        Serial.begin(9600);
        //X.attach(9);                //attach motor so Step/B is connected to pin 9
        //X.attach(8,9);              //Direction/A is pin 8, Step/B is pin 9
        //X.attach(8,9,6);            //Direction/A is pin 8, Step/B is pin 9, Enable is pin 6
        X.attach(24,4,25,3);            //Direction/A is pin 8, Step/B is pin 9, Enable is pin 6, HLFB is pin 4
      
        // Set max Velocity.  Parameter can be between 2 and 100,000 steps/sec
        X.setMaxVel(50000);
        // Set max Acceleration.  Parameter can be between 4000 and 2,000,000 steps/sec/sec
        X.setMaxAccel(200000);
        // Enable motor, reset the motor position to 0
        X.enable();
      
        delay(400);
      
        // Set up the ISR to constantly update motor position.  All motor(s) must be attached, and enabled before this function is called.
        machine.Start();
}    

void loop(){
        input = analogRead(A7);                           //read tension data from PIN A0
        // Calibrate and find the exact tension value
        if (input<=0) {
          sensorVal_cal = 0;
        } else {
          sensorVal_cal = input * 0.02 + 0.06;            //Calibration formula
        }
        //Call PID controller function and do the calculation
        output = computePID(sensorVal_cal);
        delay(1);
        X.move(output);                //control the motor based on PID value
        // Read encoder value
        HLFB_Val = X.readHLFB();
        Steps = X.calcSteps();
        Position = X.getCommandedPosition();
        // Print out any parameter; here I print Position
       // Serial.println(HLFB_Val);

        //Security orders, not to crush
        if (Position<-15000 || Position>15000) {
          X.disable();
        }
}
// PID controller function
double computePID(float 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 - lastError)/elapsedTime;                // compute derivative

        double out = kp*error + ki*cumError + kd*rateError;         //PID output               

        lastError = error;                                          //remember current error
        previousTime = currentTime;                                 //remember current time

        return out;                                                 //return the PID output
}
schema.PNG
 
Back
Top