单片机同时控制多个舵机(ESP8266)
Talk is cheap, show you code!/** @Function:control 4 servos at the same time, and by this way, theoretically, you can* control more than 4 as long as the pin numbers allowed.* @device: ESP8266* @platf
·
Talk is cheap, show you code!
/*
* @Function:control 4 servos at the same time, and by this way, theoretically, you can
* control more than 4 as long as the pin numbers allowed.
* @device: ESP8266
* @platform: Arduino IDE
* @author:Feng 2022/2/3/16:19
*/
#define SERVOPORT0 16//D0
#define SERVOPORT1 5//D1
#define SERVOPORT2 4//D2
#define SERVOPORT3 2//D4
struct SERVO
{
uint8_t port_id;
uint16_t angle;
uint16_t t;
};
SERVO servo[4];
void servo_init()
{
pinMode(SERVOPORT0,OUTPUT);
pinMode(SERVOPORT1,OUTPUT);
pinMode(SERVOPORT2,OUTPUT);
pinMode(SERVOPORT3,OUTPUT);
servo[0].port_id=SERVOPORT0;
servo[1].port_id=SERVOPORT1;
servo[2].port_id=SERVOPORT2;
servo[3].port_id=SERVOPORT3;
}
void setup() {
// put your setup code here, to run once:
servo_init();
Serial.begin(115200);
}
void loop() {
// put your main code here, to run repeatedly:
int angle1,angle2,angle3,angle4;
for(int i=0;i<=180;i++)
{
angle1=i;
angle2=180-i;
angle3=i;
angle4=180-i;
servo_angle(angle1,angle2,angle3,angle4);
}
}
void servo_angle(uint16_t angle1,uint16_t angle2,uint16_t angle3,uint16_t angle4)
{
uint16_t i,j;
SERVO temp,servo_copy[4];
servo[0].angle=angle1;
servo[1].angle=angle2;
servo[2].angle=angle3;
servo[3].angle=angle4;
for(i=0;i<4;i++)
{
servo[i].t = 11.1111*servo[i].angle+500;
}
for(i=0;i<4;i++)
{
servo_copy[i] = servo[i];
}
for(i=0;i<3;i++)
{
for(j=i+1;j<4;j++)
{
if( servo_copy[i].angle > servo_copy[j].angle )
{
temp = servo_copy[i];
servo_copy[i] = servo_copy[j];
servo_copy[j] = temp;
}
}
}
//Serial.printf("%d,%d,%d, %d,%d,%d, %d,%d,%d, %d,%d,%d\n",servo_copy[0].port_id,servo_copy[0].angle,servo_copy[0].t, servo_copy[1].port_id,servo_copy[1].angle,servo_copy[1].t, servo_copy[2].port_id,servo_copy[2].angle,servo_copy[2].t, servo_copy[3].port_id,servo_copy[3].angle,servo_copy[3].t);//Debug
byte count=20;
while(count--)
{
digitalWrite(SERVOPORT0,HIGH);
digitalWrite(SERVOPORT1,HIGH);
digitalWrite(SERVOPORT2,HIGH);
digitalWrite(SERVOPORT3,HIGH);
delayMicroseconds(servo_copy[0].t);
digitalWrite(servo_copy[0].port_id,LOW);
delayMicroseconds(servo_copy[1].t-servo_copy[0].t);
digitalWrite(servo_copy[1].port_id,LOW);
delayMicroseconds(servo_copy[2].t-servo_copy[1].t);
digitalWrite(servo_copy[2].port_id,LOW);
delayMicroseconds(servo_copy[3].t-servo_copy[2].t);
digitalWrite(servo_copy[3].port_id,LOW);
delayMicroseconds(20000-servo_copy[3].t);
}
}
更多推荐
已为社区贡献2条内容
所有评论(0)