Und hier ist mein Script:
//**********
#include <Servo.h>
#define FRONTLEFTSERVOPIN 3
#define FRONTRIGHTSERVOPIN 11
#define BACKLEFTSERVOPIN 6
#define BACKRIGHTSERVOPIN 9
Servo frontLeftServo;
Servo frontRightServo;
Servo backLeftServo;
Servo backRightServo;
int robotSpeed = 100;
int rightSpeed = 100;
int leftSpeed = 100;
int delayParam = 50;
void setup()
{
robotSetup();
}
void loop()
{
goForward();
delay(delayParam * 100);
goStop();
delay(delayParam * 100);
goBackward();
delay(delayParam * 100);
goStop();
delay(delayParam * 100);
goLeft();
delay(delayParam * 100);
goStop();
delay(delayParam * 100);
goRight();
delay(delayParam * 100);
goStop();
delay(delayParam * 100);
}
void robotSetup(){
setSpeed(robotSpeed);
pinMode(FRONTLEFTSERVOPIN, OUTPUT);
pinMode(FRONTRIGHTSERVOPIN, OUTPUT);
pinMode(BACKLEFTSERVOPIN, OUTPUT);
pinMode(BACKRIGHTSERVOPIN, OUTPUT);
frontLeftServo.attach(FRONTLEFTSERVOPIN);
frontRightServo.attach(FRONTRIGHTSERVOPIN);
backLeftServo.attach(BACKLEFTSERVOPIN);
backRightServo.attach(BACKRIGHTSERVOPIN);
goStop();
}
void setSpeed(int newSpeed)
{
setSpeedLeft(newSpeed);
setSpeedRight(newSpeed);
}
void setSpeedLeft(int newSpeed)
{
if(newSpeed >= 100)
{
newSpeed = 100;
}
if(newSpeed <= 0)
{
newSpeed = 0;
}
leftSpeed = newSpeed * 0.65;
}
void setSpeedRight(int newSpeed)
{
if(newSpeed >= 100)
{
newSpeed = 100;
}
if(newSpeed <= 0)
{
newSpeed = 0;
}
rightSpeed = newSpeed * 0.65;
}
void goForward()
{
frontLeftServo.write(65 + leftSpeed);
frontRightServo.write(65 - rightSpeed);
backLeftServo.write(67 + leftSpeed);
backRightServo.write(64 - rightSpeed);
}
void goBackward()
{
frontLeftServo.write(65 - leftSpeed);
frontRightServo.write(65 + rightSpeed);
backLeftServo.write(66 - leftSpeed);
backRightServo.write(64 + rightSpeed);
}
void goRight()
{
frontLeftServo.write(65 + leftSpeed);
frontRightServo.write(65 + rightSpeed);
backLeftServo.write(66 + leftSpeed);
backRightServo.write(64 + rightSpeed);
}
void goLeft()
{
frontLeftServo.write(65 - leftSpeed);
frontRightServo.write(65 - rightSpeed);
backLeftServo.write(66 - leftSpeed);
backRightServo.write(64 - rightSpeed);
}
void goStop()
{
frontLeftServo.write(65);
frontRightServo.write(65);
backLeftServo.write(66);
backRightServo.write(64);
}
//**********