Hi, i am making a tethering system for a drone project by using a arduino motor shield with geared motor encoder. The main objective of the tethering system is to control the tension of the string that is being tied to the drone. It works similar to a fishing reel, it releases then pulls back the string to keep the line taut. However, it doesn't work what could be the problem?
The following are the specifications of the geared motor with encoder and motor shield
https://store.arduino.cc/usa/arduino-motor-shield-rev3
The followings are my code for the motor encoder and motor shield
const byte phaseA=7; // digital pin
const byte phaseB=4; //digital pin
volatile int count,intcount,secs;
volatile int newCount, oldCount, Mspeed, MsetSpeed=10, errSpeed, errSpeedRate, oldErrSpeed, sumErrSpeed, speedOut;
volatile int kp=3,kd=1,ki=1,value;
void setup() {
// put your setup code here, to run once:
pinMode(phaseA,INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(phaseA),phaseAisr,CHANGE);
pinMode(phaseB,INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(phaseB),phaseBisr,CHANGE);
Serial.begin(9600);
Serial.print("Enter set speed: ");
Serial.println(MsetSpeed);
Serial.print("Enter set speed: ");
TIMSK2=TIMSK2 | 0x01;
pinMode(12,OUTPUT); // DIR A pin
pinMode(3,OUTPUT); // PWM pin
}
void loop() {
// put your main code here, to run repeatedly:
value=Serial.parseInt();
if(value!=0){
MsetSpeed=value;
Serial.println(value);
Serial.print("Enter set Speed: ");
}
}
ISR(TIMER2_OVF_vect,ISR_BLOCK){
intcount++;
if(intcount%500==0){
secs++;
}
newCount=count;
Mspeed=newCount-oldCount;
oldCount=newCount;
errSpeed=MsetSpeed-Mspeed;
errSpeedRate=errSpeed-oldErrSpeed;
oldErrSpeed=errSpeed;
sumErrSpeed=sumErrSpeed+errSpeed;
speedOut=(kp*errSpeed)+(kd*errSpeedRate)+(ki*sumErrSpeed);
if(speedOut>255){
speedOut=255;
}
else if(speedOut<-255){
speedOut=-255;
}
if(speedOut<0){
speedOut=-speedOut;
digitalWrite(12,HIGH); //motor driver pins
}
else{
digitalWrite(12,LOW); //motor driver pins
}
analogWrite(3,speedOut); //PWM pin
}
void phaseAisr(){
if(digitalRead(phaseA)==HIGH){
if(digitalRead(phaseB)==LOW){
count++;
}
else{
count--;
}
}
else{
if(digitalRead(phaseB)==LOW){
count--;
}
else{
count++;
}
}
}
void phaseBisr(){
if(digitalRead(phaseB)==HIGH){
if(digitalRead(phaseA)==LOW){
count--;
}
else{
count++;
}
}
else{
if(digitalRead(phaseA)==LOW){
count++;
}
else{
count--;
}
}
}