Felaktig Arduino kod ger nervöst servo
Har konverterat en gammaldags hastighetsmätare till servodrift men får det inte att fungera så bra. Använder mig av ett Arduino Nano kort som jag programmerat med hjälp av modifierad kod från likande projekt. Servot är dock väldigt nervöst. En induktiv givare ger insignalen via en optokopplare, har dock inte möjlighet att kika på råsignalen men efter optokopplaren borde den vara okej. Servot är ett 270 graders från Kjell.co. Jag har en "stroke" funktion då servot går från 0-270 grader och tillbaks utan bekymmer, men då jag kör fordonet den sitter i fladdrar servot hej vilt. Har hittat olika medelsnitt och filtreringskoder, men jag saknar kompetensen för att framgångsrikt kunna implementera det i min kod. Jag vet inte heller om min kod är felaktigt eller om det helt enkelt krävs nån form av dämpning av in eller utsignalen.
Finns det nån som kan hjälpa mig med att antingen implementera en lösning eller hitta felet i koden?
Tack!
Magnus
/*
Speedometer
Stolen from other designers (open source) 31 Jan 2021
by Magnus Eriksson
*/
// include the Servo library
#include <Servo.h>
Servo myServo; // create a servo object
unsigned int min_speed = 0; // minimal speed to display on scale, km/h ********ta bort???
unsigned int max_speed = 135; // maximum speed to display on scale, km/h******ta bort??
float start, finished;
float elapsed, time;
float circMetric=0.32; // wheel circumference (in meters)/number of pulses per turn: 1.92/6=0.32
float speedk; // holds calculated speed values
int angle; // variable to hold the angle for the servo motor
void setup() {
Serial.begin(9600); // configure serial for debug
myServo.attach(4); // attaches the servo on pin 4 to the servo object
attachInterrupt(digitalPinToInterrupt(2), speedCalc, RISING); // interrupt called when sensors sends digital 2 high (every wheel rotation)
//start now (it will be reset by the interrupt after calculating revolution time)
start=millis();
myServo.write(269); //Stroke test
delay(1300);
myServo.write(0); //Stroke test
delay(1300);
}
void speedCalc()
{
//Function called by the interrupt
if((millis()-start)>1) // 5 millisec debounce
{
//calculate elapsed
elapsed=millis()-start;
//reset start
start=millis();
//calculate speed in km/h
speedk=(3600*circMetric)/elapsed; //(((circMetric/elapsed)/1000)/1000)*3600
Serial.print(", Speedk: ");
Serial.println(speedk);
}
}
void loop()
{
// The loop will be interrupted by the sensor each time the
// magnet passes near the sensor, in other words six times per revolution
// convert speedk value to servo angle
angle = map(speedk, 0, 135, 269, 0); //135 istället för 140 då servot inte går längre
Serial.print(", angle: ");
Serial.println(angle);
// set the servo position
myServo.write(angle);
// wait for the servo to get there
delay(500);
if ((millis() - start) > 2000) { // if there is no signal more than 2 seconds
speedk = 0; // so, speed is 0
}
}
