Hi guys !
I'm working on a project an I'm totally stuck. I must programm a PID speed controller with specific material.
I have at my disposal :
- a YUMO E6B2-CWZ3E encoder (which you can see here Rotary Encoder - 1024 P/R (Quadrature) - COM-11102 - SparkFun Electronics)
- a PM25R-45F-1003 motor (which you can see here https://www.andymark.com/CIM-Motor-p/am-0255.htm)
- a VICTOR SP motor controller (which you can see here https://www.vexrobotics.com/217-9090.html)
- an Arduino Mega 2560
- a 12V battery
I made the following connexions : (see attached image)
And I wrote this code :
#include <PID_v1.h> // For the PID
#include <TimerOne.h> //For the timer
#define ENCODEURA 2
#define ENCODEURB 4
#define A 5 // Speed control for the motor
double Setpoint, Input, Output;
double Kp=0.8, Ki=10, Kd=0.1;
PID PID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
volatile int count =0; // encoder's ticks counting
volatile double speed =0; // motor's speed
volatile unsigned long time0=0; // get a time value at a given time
volatile unsigned long timer=0; // a value which will contain the last time gotten via millis
volatile byte laststate =0; // last encoder's state
volatile int commande=200; // order for the motor (between 0 and 255)
int valeur; //order's reading via the monitor
void setup()
{
//monitor's initialization
Serial.begin(9600); // here for feedback
// Inputs and Outputs initialization
pinMode(ENCODEURA, INPUT_PULLUP);
pinMode(ENCODEURB, INPUT_PULLUP);
pinMode(A, OUTPUT);
// Encoder quadrature counter
attachInterrupt(0,counter, CHANGE);
Timer1.initialize(100000); // set a timer of length 100000 microseconds
//(or 0.1 sec - or 10Hz => the led will blink 5 times, 5 cycles of on-and-off, per second)
Timer1.attachInterrupt( timerIsr );
// PID
Input = 0;
Setpoint = 0;
//activate the PID
PID.SetMode(AUTOMATIC);
PID.SetOutputLimits(-255, 255);
}
void loop(){
if(Serial.available()){
valeur=Serial.parseInt();
if(valeur!=0){
commande = valeur;
}
}
Setpoint=commande;
Input=speed;
PID.Compute();
int output=(int)Output;
Serial.println("commande");
Serial.println(commande);
Serial.print("'''''''''''''''''");
Serial.println("speed");
Serial.println(speed);
Serial.print("'''''''''''''''''");
Serial.println("output");
Serial.println(output);
Serial.print("'''''''''''''''''"); //only here for feedback
//using the PID Output in order to control the motor
if(output>=0){
advance(255-output);
}
timer = millis();
if( (timer-time0)>10000)
{
commande= 0;
time0=timer;
//Timeout !
}
}
void advance(int a) // Let's go !
{
analogWrite (A,a); // PWM speed control
}
// Encoder counter 0
void counter()
{
byte state=PIND;
state|=B11101011; // mask to look at the changes only on PIN 2 and 4
if( state!=laststate)
{
(((state&(1<<ENCODEURA))>>ENCODEURA)^((state&(1<<ENCODEURB))>>ENCODEURB))?count--:count++;
laststate=state;
}
}
// Timer in order to calculate the speed with the encoders
void timerIsr()
{
speed=count;
count=0;
}
The problem is : nothing works ! I don't know why but the motor doesn't turn.
I tried many different things and read many tutorials but I can't find a way to make this work.
Could a good Samaritan help a man in need ?
Thanks !
