Hi,
I have two boards linked per radio, one sending a value from a joystick and the other moving a servo according to the value.
But the servo doesn’t move when I move the joystick fast to one direction and hold it. For instance, if I move the joystick to the right and hold it in full right position, the servo will stop at say 40 degrees. If I move slightly the joystick, the servo move finaly to 180 degrees.
I checked the received value, it is allright. The code “myservo.write(radioValue);” is simply not applied when the joystick move fast. Any guess?
#define ISKITEVERSION
//#define ISBASEVERSION
// comment out one or other above
const int ledPin=13; // number of the pin with onboard LED
const byte telegrammstart = 255; // start of 3-byte telegram
const int delayValue = 50; // delay between 2 telegrams to send
const unsigned long unsignedLongAlmostMax = 4294000000; // close to overflow of millis
char radioModus = 'A'; // A: Joystick modus, B: 8-fly/pull, C: stable
int radioValue = 0; // value to be transmitted
int incomingByte = 0;
int sensorValue = 0; // variable to store the value coming from the sensor
unsigned long millisLast = 0;
#ifdef ISKITEVERSION
#include <Servo.h>
Servo myservo;
int servoPos = 0;
int servoPin = 8;
//The PID code comes next
#include <PID_v1.h>
//The code for the IMU+servo
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Servo.h"
MPU6050 mpu;
int16_t ax, ay, az;
int16_t gx, gy, gz;
int Val, Target;
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint, 1.5,0,0, REVERSE);
#endif
#ifdef ISBASEVERSION
const int sensorPin = A0; // select the input pin for the potentiometer
int modePin1 = 2;
int modePin2 = 3;
int modePin3 = 4;
char modeSelect()
{
if (digitalRead(modePin1)==LOW) return 'A';
if (digitalRead(modePin2)==LOW) return 'B';
if (digitalRead(modePin3)==LOW) return 'C';
}
#endif
void setup()
{
Serial.begin(9600);
pinMode(ledPin, OUTPUT);
millisLast = 0;
#ifdef ISKITEVERSION
myservo.attach(servoPin);
//initialize the variables we're linked to
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
Input = map(ay, -17000, 17000, 0, 255);
Setpoint = 90;
//turn the PID on
myPID.SetMode(AUTOMATIC);
//Hereafter the code for the IMU+servo
Wire.begin();
Serial.begin(9600);
Serial.println("Initialize MPU");
mpu.initialize();
Serial.println(mpu.testConnection() ? "Connected" : "Connection failed");
myservo.attach(8);
myPID.SetOutputLimits(-90, 90);
#endif
#ifdef ISBASEVERSION
pinMode(modePin1, INPUT_PULLUP);
pinMode(modePin2, INPUT_PULLUP);
pinMode(modePin3, INPUT_PULLUP);
#endif
}
void loop()
{
#ifdef ISBASEVERSION
// read a value and send a telegram
if ((millis() >= millisLast + delayValue)) {
// if ((millis() >= millisLast + delayValue)||(millis() < millisLast - unsignedLongAlmostMax)) {
sensorValue = analogRead(sensorPin);
//value for radioModus is to be evaluated, at present only joystick control
sensorValue = map(sensorValue, 0, 1023, 0, 179);
//start with telegram
Serial.write(telegrammstart); // start of telegram
Serial.write(modeSelect()); // joystick-modus senden
Serial.write(sensorValue);
millisLast = millis();
}
#endif
// ==============================================
// see if there's incoming serial data sent
if (Serial.available() > 2) {
// minimum 3 bytes in buffer (sequence telegramstart, modus, value)
incomingByte = Serial.read();
if (incomingByte == telegrammstart) {
radioModus = Serial.read();
radioValue = Serial.read();
#ifdef ISKITEVERSION
if(radioModus=='A') {
// joystick mode
myservo.write(radioValue); // sets the servo position according to the scaled value
Serial.println(radioValue);
//return;
} else if (radioModus=='B') {
//Hereafter the code for the IMU+servo
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
Val = map(ay, -17000, 17000, 1, 180);
Input = (double)Val;
myPID.Compute();
Target = Output + Setpoint ;
myservo.write(Target);
//Print some values for debug
//Serial.print("Setpoint: ");
//Serial.print(Setpoint);
//Serial.print("ay: ");
//Serial.print(ay);
//Serial.print(" Val: ");
//Serial.print(Val);
//Serial.print(" Intput: ");
//Serial.print(Input);
//Serial.print(" Output: ");
//Serial.print(Output);
//Serial.print(" Target: ");
//Serial.println(Target);
//delay(10);
//return;
} else if (radioModus=='C') {
myservo.write(180); // test to show modus
}
#endif
}
}
}