Servo Feedback hacka (gratis) (8 / 8 steg)

Steg 8: Auto inställning

jag satte ihop en mer komplett kodexempel baserat på V2 från steg 7. Det kommer att automatiskt ange de två klädd servon, spara intervall data i EEPROM (feedback utbud och oss puls intervall) för framtida omstart. Det finns också en möjlighet att återställa data sparade utbud på omstart.
Det finns också en trevlig block som genererar smidig servo acceleration och retardation baserat på cosinusfunktion. Måste älska trig.
Njut!

/ * > Auto_Set_Range <
Testa kod för servo med hackade feedback från inre potten.
Servo kontroll kabel ansluten till digital 10 och 11
Ståndpunkten Feedback kabeln ansluten till analoga 0 och 1
Servon är declaired i en matris för lätt tillägg av mer servon.
Bygga en spänningsavdelare på ARef stift: + 5V--4.7K--Aref--3.3K--GND
Med en reglerad spänning på 4.78V kommer han som följer analog referens att ~1.85V
(se http://arduino.cc/en/Reference/AnalogReference)
Detta kommer att öka upplösningen på potentiometern läsning.
Använd följande kommandon till toggel mellan att läsa servo feedback
och läsa andra analoga stift som behöver se 5V som referens
analogReference(EXTERNAL); anger analoga 1023 till spänning på ARef stift
analogReference(DEFAULT); anger analoga 1023 till 5V eller 3.3V beroende på strömförsörjning

DENNA KOD KOMMER IN PÅ 8,9 K MINNE. UTAN ALL FÖLJETONG FEEDBACK ÄR DET 8.3K
DET KAN STÅ ATT MINSKAS MED NÅGON SMARTARE ÄN MIG, JAG ÄR SÄKER!

*/

#include < Servo.h > //import servo bibliotek
#include < EEPROM.h > //import EEPROM bibliotek
CONST int numServos = 2; Hur många servon har du?
CONST int sPin [numServos] = {10,11}. vilka stift de korrelerar till?
Servo servo [numServos]; deklarera arrayen servo

int highPulse [numServos]; hög puls bredd
int lowPulse [numServos]; låg puls bredd
int en [numServos]; lowPulse feedback behandlingen
int B [numServos]; highPulse feedback behandlingen
int här [numServos]; vinkel från
int det [numServos]; vinkel flytta till

float x [numServos]; vinkel konverteras till radianer att härleda cosinus våg

int h;
int t;
int feedBack; används för att hålla servo feedback värde
int knappen = 2;
int e = 1; EEPROM adress att börja lagra/datahämtning servo
int metar; vinkel som härrör från cosinusfunktion. skickas till servo i loop
int btwReadings = 20; fördröjningstid mellan
int whlReading = 3; fördröjningstid mellan analoga behandlingen av inre potten

booleska rangeTest = false;
booleska doneMove [numServos];

void setup() {

Serial.BEGIN(19200); initiera serial output
Serial.println ("det är!");
analogReference(EXTERNAL);
pinMode (knapp, indata);
för (int jag = 0; jag < numServos; i ++) {
pinMode (sPin[i],OUTPUT);
}
int n = EEPROM.read(0);
om (n == 20) {
callRange();
}
om (rangeTest == false) {
för (int jag = 0; jag < numServos; i ++) {
setRange(i); gå testa och ange värden
doneMove [i] = true;
}
rangeTest = sant;
EEPROM.write(0,20); Ange framtida startups att vi har gjort detta!
}
Delay(1000);
}

void loop() {

för (int jag = 0; jag < numServos; i ++) {
om (doneMove [i] == true) {
doneMove [i] = false;
här [i] = det [i];
det [i] = random (180.1) + 0,5;
om (där [i] == here[i]) {där [i] = random (180.1) + 0,5;}
om (här [i] < there[i]) {x [i] = 0;} annat {x [i] = 180;}
Serial.Print ("flytta servo");
Serial.Print(i);
Serial.Print ("från");
Serial.Print(here[i]);
Serial.Print ("till");
Serial.println(There[i]);
}
}

calcCos (nuvarande position, önskat läge, steg, servo array position)
för (int jag = 0; jag < numServos; i ++) {
vinkel = calcCos(here[i],there[i],1.5,i);
om (doneMove [i] == false) {
servo[i].write(Angle);
Delay(5);
}
}
} / / END OGILTIG LOOP

/*
DETTA FUNKTIONEN AUTO-ANGER INTERVALLET SERVO
UTGÅR FRÅN TRYCKKNAPP PÅ PIN 2
*/

void setRange(int x) {//parameter passerade är array position av servo
int pb = 0; används för att hålla knappen behandlingen
int-test. allmän användning variabel
int h;
int t;
int puls = 1500; först oss puls används i olika test
Serial.Print ("Tryck för att ställa in intervall för Servo[");
Serial.Print(x);
Serial.println("].");
While(!PB) {
PB = digitalRead(Button);
}
PB = 0;
Serial.Print ("Ange rad begränsningar i... 3");
för (int jag = 2; jag > = 0; i--) {//count ner tre sekunder
Delay(1000);
Serial.Print("..");
Serial.Print(i);
}
Serial.println();
servo[x].attach(sPin[x]);
Delay(20);
servo[x].writeMicroseconds(Pulse); Skicka servo till mitten av intervallet
Delay(2000); vänta att få det

göra {
puls + = 10. incriment oss puls bredd
readMove(x,pulse);
} while(h > t); förutsättning för att hålla provning rad
highPulse [x] = pulse-20; Håll dig borta från rad extrema
B [x] = h-10; justera feedback från extreme
Serial.println();
servo[x].writeMicroseconds(highPulse[x]);
puls = highPulse [x];
Delay(500);
göra {
Pulse-=10;
readMove(x,pulse);
} while(h < t);

lowPulse [x] = puls + 20.
En [x] = t + 10.
servo[x].writeMicroseconds(lowPulse[x]);

feedBack = getFeedback(x); ta nuvarande läsning från potten
där [x] = map(feedBack,A[x],B[x],0,180); justera feedback till grad utgång
servo[x].attach(sPin[x],lowPulse[x],highPulse[x]); bifoga detta servo
servo[x].write(There[x]); skicka ut puls för där vi är

doneMove [x] = true;

testa = [x] >> 8.
writeE(test); lagra låg feedback behandlingen
writeE(A[x]);
testa = B [x] >> 8.
writeE(test); lagra höga feedback behandlingen
writeE(B[x]);
testa = lowPulse [x] >> 8.
writeE(test); lagra låg kontroll puls
writeE(lowPulse[x]);
testa = highPulse [x] >> 8.
writeE(test); lagra kick kontrollerar puls
writeE(highPulse[x]);

Serial.println ("Feedback utbud:");
Serial.Print(A[x]);
Serial.Print ("<>");
Serial.println(B[x]);
Serial.println ("oss puls intervall:");
Serial.Print(lowPulse[x]);
Serial.Print ("<>");
Serial.println(highPulse[x]);
Serial.Print("servo[");
Serial.Print(x);
Serial.println ("] fäst, data som sparas i EEPROM");

} //end setRange()

void writeE (byte b) {
EEPROM.write(e,b);
e + = 1;
}

void readMove (int n, int p) {
t = getFeedback(n);
servo[n].writeMicroseconds(p);
Delay(btwReadings);
h = getFeedback(n);
Serial.println(h);
} //END UPPSÄTTNING UTBUD

/*
DENNA FUNKTION LÄSER INTERN SERVO POTENTIOMETERN
*/
int getFeedback(int a) {
int j;
int menar;
int resultatet;
int-test.
int läsa [20].
Boolean gjort;

för (j = 0; j < 20; j ++) {
läsa [j] = analogRead(a); få rådata från servo potentiometer
Delay(whlReading);
} / / sortera arrayen avläsningar låg till hög i
= false; Rensa sortering flagga
medan (gjort! = sant) {/ / enkel swap sortera, sorterar siffror från lägsta till högsta
= sant;
för (j = 0; j < 20; j ++) {
om (läst [j] > läsa [j + 1]) {/ / sortering nummer här
testa = läsning [j + 1];
läsa [j + 1] = läsning [j];
läsa [j] = test;
= false;
}
}
}
menar = 0;
för (int k = 6; k < 14; k ++) {//discard 6 högsta och lägsta 6 avläsningar
menar += läsning [k];
}
resultat = medelvärdet/8. genomsnittliga användbara mätvärden
Return(Result);
} / / END FÅ FEEDBACK

/*
DENNA FUNKTION KRÄVER TIDIGARE UPPSÄTTNING UTBUD FRÅN EEPROM
UTGÅR FRÅN TRYCKKNAPP PÅ PIN 2
*/
void callRange() {
int-test.

Serial.Print ("att sparade urval tryck på återställningsknappen på pin 2");
för (int jag = 5; jag > = 0; i--) {
Serial.Print("..");
Serial.Print(i);
för (int j = 0; j < 100; j ++) {
om (digitalRead(Button) == 1) {
Serial.println();
Delay(1000);
hemkomst.
}
Delay(10);
}
}
Serial.println();
Serial.println ("inställningarna för servo data");
för (int jag = 0; jag < numServos; i ++) {
testa = readE();
En [i] = test << 8. får lagrade låg feedback läsning
En [i] = en [i] + readE();
testa = readE();
B [i] = test << 8. får lagrade hög feedback läsning
B [i] = B [i] + readE();
testa = readE();
lowPulse [i] = test << 8. få storeed låg kontroll puls
lowPulse [i] = lowPulse [i] + readE();
testa = readE();
highPulse [i] = test << 8. få lagrade kick kontrollerar puls
highPulse [i] = highPulse [i] + readE();
feedBack = getFeedback(i); ta nuvarande läsning från potten
det [i] = map(feedBack,A[i],B[i],0,180); justera feedback till grad utgång
servo[i].attach(sPin[i],lowPulse[i],highPulse[i]); bifoga detta servo
servo[i].write(There[i]); skicka ut puls för där vi är
doneMove [i] = true; Ställ in för att göra först flytta
Serial.println ("Feedback utbud:");
Serial.Print(A[i]);
Serial.Print ("<>");
Serial.println(B[i]);
Serial.println ("oss puls intervall:");
Serial.Print(lowPulse[i]);
Serial.Print ("<>");
Serial.println(highPulse[i]);
Serial.Print("servo[");
Serial.Print(i);
Serial.println ("] fäst, data Hämtad från EEPROM");
Serial.Print ("servo");
Serial.Print(i);
Serial.Print ("nuvarande position =");
Serial.println(There[i]);
Serial.println();

}

rangeTest = sant; Ange rangeTest-flaggan
} //end callRange()

byte readE() {
byte E = EEPROM.read(e);
e + = 1;
tur E;
} //END SAMTAL UTBUD

/*
DENNA FUNKTION SKAPAR SLÄT (COSINUS) RÖRELSE HÄRIFRÅN TILL DÄR
*/

int calcCos (int h, int th, float s, int n) {
int r;
int a;
IF(h < th) {
x [n] + = s;
om (x [n] > = 181) {doneMove [n] = true;}
r = (cos(radians(x[n]))*100);
en = map(r,100,-100,h,t);
}
IF(h > th) {
x [n]-= s;
om (x [n] < = -1) {doneMove [n] = true;}
r = (cos(radians(x[n]))*100);
en = map(r,-100,100,h,t);
}
returnera en;
} //END CALC COS