fix servo control
parent
6602842e3d
commit
923b05ffd2
|
@ -9,7 +9,13 @@ D11 D6 D5 D4
|
||||||
|
|
||||||
static ServoControl *instance;
|
static ServoControl *instance;
|
||||||
|
|
||||||
Servo servos[2];
|
#if defined(RAMPS_V14)
|
||||||
|
Servo servos[2];
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if defined(FARMDUINO_V10) || defined(FARMDUINO_V14)
|
||||||
|
Servo servos[4];
|
||||||
|
#endif
|
||||||
|
|
||||||
ServoControl *ServoControl::getInstance()
|
ServoControl *ServoControl::getInstance()
|
||||||
{
|
{
|
||||||
|
@ -28,6 +34,20 @@ int ServoControl::attach()
|
||||||
{
|
{
|
||||||
servos[0].attach(SERVO_0_PIN);
|
servos[0].attach(SERVO_0_PIN);
|
||||||
servos[1].attach(SERVO_1_PIN);
|
servos[1].attach(SERVO_1_PIN);
|
||||||
|
#if defined(FARMDUINO_V10) || defined(FARMDUINO_V14)
|
||||||
|
servos[2].attach(SERVO_2_PIN);
|
||||||
|
servos[3].attach(SERVO_3_PIN);
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
void ServoControl::detachServos()
|
||||||
|
{
|
||||||
|
servos[0].detach();
|
||||||
|
servos[1].detach();
|
||||||
|
#if defined(FARMDUINO_V10) || defined(FARMDUINO_V14)
|
||||||
|
servos[2].detach();
|
||||||
|
servos[3].detach();
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
int ServoControl::setAngle(int pin, int angle)
|
int ServoControl::setAngle(int pin, int angle)
|
||||||
|
@ -52,11 +72,27 @@ int ServoControl::setAngle(int pin, int angle)
|
||||||
switch (pin)
|
switch (pin)
|
||||||
{
|
{
|
||||||
case 4:
|
case 4:
|
||||||
|
servos[0].attach(SERVO_0_PIN);
|
||||||
servos[0].write(angle);
|
servos[0].write(angle);
|
||||||
break;
|
break;
|
||||||
case 5:
|
case 5:
|
||||||
|
servos[1].attach(SERVO_1_PIN);
|
||||||
servos[1].write(angle);
|
servos[1].write(angle);
|
||||||
break;
|
break;
|
||||||
|
case 6:
|
||||||
|
#if defined(RAMPS_V14)
|
||||||
|
return 1;
|
||||||
|
#endif
|
||||||
|
servos[2].attach(SERVO_2_PIN);
|
||||||
|
servos[2].write(angle);
|
||||||
|
break;
|
||||||
|
case 11:
|
||||||
|
#if defined(RAMPS_V14)
|
||||||
|
return 1;
|
||||||
|
#endif
|
||||||
|
servos[3].attach(SERVO_3_PIN);
|
||||||
|
servos[3].write(angle);
|
||||||
|
break;
|
||||||
default:
|
default:
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
|
@ -20,6 +20,7 @@ public:
|
||||||
static ServoControl *getInstance();
|
static ServoControl *getInstance();
|
||||||
|
|
||||||
int attach();
|
int attach();
|
||||||
|
void detachServos();
|
||||||
int setAngle(int pin, int angle);
|
int setAngle(int pin, int angle);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
|
@ -434,6 +434,7 @@ void loop()
|
||||||
{
|
{
|
||||||
StepperControl::getInstance()->disableMotorsEmergency();
|
StepperControl::getInstance()->disableMotorsEmergency();
|
||||||
PinControl::getInstance()->resetPinsUsed();
|
PinControl::getInstance()->resetPinsUsed();
|
||||||
|
ServoControl::getInstance()->detachServos();
|
||||||
if (debugMessages)
|
if (debugMessages)
|
||||||
{
|
{
|
||||||
Serial.print(COMM_REPORT_COMMENT);
|
Serial.print(COMM_REPORT_COMMENT);
|
||||||
|
|
|
@ -202,7 +202,7 @@
|
||||||
#define SERVO_0_PIN 4
|
#define SERVO_0_PIN 4
|
||||||
#define SERVO_1_PIN 5
|
#define SERVO_1_PIN 5
|
||||||
#define SERVO_2_PIN 6
|
#define SERVO_2_PIN 6
|
||||||
#define SERVO_3_PIN 7
|
#define SERVO_3_PIN 11
|
||||||
|
|
||||||
// Encoder X channel A: pin 16, port H1
|
// Encoder X channel A: pin 16, port H1
|
||||||
#define ENC_X_A_PORT PINH
|
#define ENC_X_A_PORT PINH
|
||||||
|
|
Loading…
Reference in New Issue