Ich hab momentan Probleme mit meinem Code welchen ich vor einiger Zeit entwickelt habe da er seine Funktion mehrere Servo Motoren an eine Position zu bringen mit einer Funktion ähnlich wie das Sweep Beispiel und alle in der selben Zeit nicht mehr erfüllt und ich bin mir relativ sicher das er nachdem ich ihn entwickelt hatte funktioniert hat. Ich weiß der Code ist relativ lang und etwas schwer zu lesen aber ich hoffe jemand hier kann mir helfen.
#include <Servo.h>
int NumberOfMotors = 6;
Servo SM[6];
int SPin[6];
int Number;
int Break; // um eine Pause zwischen zwei Bewegungen anzugeben
boolean ToF[6]; // gibt an ob drehgeschwindigkeit oder zeitraum der drehung angegeben ist
int Unit[6]; // gibt einheit der zeit an sekunden minuten usw
int Period[6]; // gibt zeit an in der sich eine Bewegung vollziehen soll
int MSpeed[6]; // gibt direkt geschwindigkeit einer Bewegung an
int PAlt[6]; // Gibt alte Position an
int PNeu[6]; // Gibt geplante Position an
void StartAllocation(int Number2, boolean a, int b, int c, int d, int e, int f) {
ToF[Number2] = a;
Unit[Number2] = b;
Period[Number2] = c;
MSpeed[Number2] = d;
PAlt[Number2] = e;
PNeu[Number2] = f;
}
void Allocation(int Number2, boolean a, int b, int c, int d, int f) {
ToF[Number2] = a;
Unit[Number2] = b;
Period[Number2] = c;
MSpeed[Number2] = d;
PNeu[Number2] = f;
}
int MovingTime() {
int output = 0;
if (PNeu[Number] == PAlt[Number]) {
output = 0;
}
else {
if (ToF[Number] == true) {
output = MSpeed[Number] * Unit[Number];
}
if (ToF[Number] == false) {
if (PAlt[Number] < PNeu[Number]) {
output = ((Period[Number] * Unit[Number]) / (PNeu[Number] - PAlt[Number]));
}
if (PAlt[Number] > PNeu[Number]) {
output = ((Period[Number] / (PAlt[Number] - PNeu[Number])) * Unit[Number]);
}
}
}
return output;
}
void Moving() {
if (PAlt[Number] < PNeu[Number]) {
PAlt[Number]++;
}
if (PAlt[Number] > PNeu[Number]) {
PAlt[Number]--;
}
SM[Number].write(PAlt[Number]);
}
void Action() {
int TimeOld[6];
int WaitingTime[6];
int controle = 0;
Number = 0;
for (int i = 0; i < NumberOfMotors; i++) {
TimeOld[Number] = millis();
WaitingTime[Number] = MovingTime();
Number++;
}
Number = 0;
while (controle < NumberOfMotors) {
Number = 0;
for (int i = 0; i < NumberOfMotors; i++) {
if (TimeOld[Number] + WaitingTime[Number] < millis()) {
Moving();
TimeOld[Number] = millis();
}
Number++;
}
controle = 0;
Number = 0;
for (int i = 0; i < NumberOfMotors; i++) {
if (PAlt[Number] == PNeu[Number]) {
controle++;
}
Number++;
}
}
delay(Break);
}
void setup() {
Serial.begin(9600);
SPin[0] = 3;
SPin[1] = 5;
SPin[2] = 6;
SPin[3] = 9;
SPin[4] = 10;
SPin[5] = 11;
Number = 0;
for (int i = 0; i < 6; i++) {
SM[Number].attach(SPin[Number]);
Number++;
SM[0].write(90);
SM[1].write(80);
SM[2].write(80);
SM[3].write(0);
SM[4].write(90);
PAlt[0] = 90;
PAlt[1] = 80;
PAlt[2] = 80;
PAlt[3] = 0;
PAlt[4] = 90;
delay(5000);
Serial.print("Start");
Allocation(0, false, 1, 10000, 1, 85);
Allocation(1, false, 1, 10000, 1, 72);
Allocation(2, false, 1, 10000, 1, 149);
Allocation(3, false, 1, 10000, 1, 0);
Allocation(4, false, 1, 10000, 1, 138);
Action();
}
/*
Allocation(0, Boolean ToF, Unit, Period, MovingSpeed, Neue Position, Break in Millisekunden)
Folgendes verwenden um neu Bewegung in Void Setup hinzuzufügen:
Allocation(0, , , , , );
Allocation(1, , , , , );
Allocation(2, , , , , );
Allocation(3, , , , , );
Allocation(4, , , , , );
Action();
*/
}
void loop() {}
).
