// STM32 Nucleo-C031C6 I2C Example
// Simulation: https://wokwi.com/projects/365421666018061313
#include <ServoSmooth.h>
#define I2C_ADDR 0x27
#define LCD_COLUMNS 20
#define LCD_LINES 4
#define AngleX A0
#define AngleY A1
ServoSmooth servos[3];
int AngleX_got = 0, AngleY_got = 0, servo2 = 0, servo3 = 0, Angle_X, Angle_Y, Joy_X, Joy_Y;
const int AngleX_c = 511, AngleY_c = 511, servo_avg = 1167;
void setup() {
pinMode(AngleX,INPUT_ANALOG);
pinMode(AngleY,INPUT_ANALOG);
/*Серво мотор 1*/
servos[0].attach(3,500,2500); // Задаём параметры серво
servos[0].setMaxAngle(270); // Максимальный угол
servos[0].setAccel(0.8); // Ставим ускорение (плавное но меньше)
servos[0].setSpeed(81); // Ставим скорость (в соответсвии документации на серво)
servos[0].setTarget(servo_avg); // Ставим сброс на ось X(1166 == 90)
/*Серво мотор 2*/
servos[1].attach(5,500,2500);
servos[1].setMaxAngle(270);
servos[1].setAccel(0.8);
servos[1].setSpeed(81);
servos[1].setTarget(servo_avg); // Ставим сброс на ось Y
/*Серво мотор 3*/
servos[2].attach(6,500,2500);
servos[2].setMaxAngle(270);
servos[2].setAccel(0.8);
servos[2].setSpeed(81);
servos[2].setTarget(servo_avg); // Ставим сброс на ось Y
}
void loop() {
static uint32_t tim = 0;
if (millis() - tim >= 100)
{
if ((analogRead(AngleX) != AngleX_c) || (analogRead(AngleY) != AngleY_c))
{
AngleX_got = map(analogRead(AngleX),0,1023,833,1500);
AngleY_got = map(analogRead(AngleY),0,1023,500,1167);
Joy_X = map(analogRead(AngleX),0,1023,0,180);
Joy_Y = map(analogRead(AngleY),0,1023,0,180);
/* code */
servos[0].setTarget(AngleX_got);
servo2 = (servo_avg-AngleY_got>=500)?servo_avg-AngleY_got:500;
servo3 = (servo_avg+AngleY_got<=1833)?servo_avg+AngleY_got:1833;
servos[1].setTarget(servo2);
servos[2].setTarget(servo3);
}
tim = (tim < UINT32_MAX)?millis():0;
}
}