I finally had the first test run of my rc chopper control. It was not as promising as I hoped.
Steering the servos with the joystick worked, as did controlling the rotor throttle. Also the sensor data was received well on the netbook and it visualised perfectly.
Then i realised something strange:
When I increase the throttle above a certain value, the servos first start to jump around, and then move to one of their extreme positions.
If I disconnect the motors, the effect vanishes. So, I suspect the problem has an electronic source.
Where should I start troubleshooting?
Here are the relevant parts of the sketch. It didn't all fit into the post.
/*
Chopper control. Replaces the receiver unit of an rc helicopter.
At first it provieds sensor data to a remote controlling unit while
executing the commands received over the bluetooth link.
*/
#define USE_COMPASS
#define USE_ACCELERO
//#define USE_BARO
#define USE_DRIVEMOTOR
#define USE_RANGE_SCANNER
//#define USE_HELP_MSG
#include <Servo.h>
#include <Wire.h>
#ifdef USE_HELP_MSG
#include <avr/pgmspace.h>
#endif
#include "Streaming.h"
#ifdef USE_RANGE_SCANNER
#include "RangeScanner.h"
#endif
#ifdef USE_BARO
#include "IntersemaBaro.h"
#endif
#ifdef USE_COMPASS
#include "PID_Beta6.h"
#endif
// analog pins
static const uint8_t pinIRDist = 3; // analog
// 4 // i2c SDA
// 5 // i2c SCL
// digital pins
// 0 // rx serial over bluetooth
// 1 // tx serial over bluetooth
static const uint8_t pinSensorPwr = 2; // power for compass, accelerometer and barometer
static const uint8_t pinBaroMCLK = 3; // the tone function used to generate the 34kHz signal interferes with pin 3 and 11, so it's best to use it directly
static const uint8_t pinBaroDOUT = 4; // in on the arduino
static const uint8_t pinMotorUpper = 5; // upper rotor throttle -> needs to be PWM
static const uint8_t pinMotorLower = 6; // lower rotor throttle -> needs to be PWM
// 7 // bluetooth reset, don't use!!!
static const uint8_t pinBaroSCLK = 8; // serial clock for the barometer
static const uint8_t pinBaroDIN = 9; // out on the arduino
static const uint8_t pinServoIrRng = 10; // servo for swaping the ir proximity sensor
// 11 // affected by the tone lib (don't use for PWM)
static const uint8_t pinServoFB = 12; // servo for forward backward tilt
static const uint8_t pinServoLR = 13; // servo for left right tilt
// i2c addresses
static const uint8_t addrCompas = (0x42 >> 1);
static const uint8_t addrNunchuck = 0x52;
#ifdef USE_RANGE_SCANNER
RangeScanner rangescn(pinIRDist, pinServoIrRng);
#endif
#ifdef USE_BARO
BaroPressure_MS5534C baro(pinBaroMCLK, pinBaroSCLK, pinBaroDIN, pinBaroDOUT);
#endif
#ifdef USE_DRIVEMOTOR
Servo srvFrontBack;
Servo srvLeftRight;
#endif
#ifdef USE_COMPASS
double pidSetpoint, pidInput, pidOutput;
PID myPID(&pidInput, &pidOutput, &pidSetpoint, 2, 5, 1);
#endif
bool doSendSerial = false;
bool doSweepServo = false;
size_t loopDelay = 100;
uint8_t targetHeading = 0;
uint8_t motorSpeed = 0;
long loopCounter = 0;
size_t evalSerialCmd();
void evalCmdStr(char *txt);
void setup()
{
pinMode(pinSensorPwr, OUTPUT);
digitalWrite(pinSensorPwr, LOW);
delay(200);
digitalWrite(pinSensorPwr, HIGH);
delay(200);
Serial.begin(115200); // arduino bt has only this baud rate
Wire.begin(); // join i2c bus as master
#ifdef USE_DRIVEMOTOR
pinMode(pinMotorUpper, OUTPUT);
pinMode(pinMotorLower, OUTPUT);
srvFrontBack.attach(pinServoFB);
srvLeftRight.attach(pinServoLR);
#endif
#ifdef USE_RANGE_SCANNER
rangescn.begin();
rangescn.setStepAngle(5);
rangescn.setScanningRange(160);
rangescn.moveToIdlePos();
#endif
#ifdef USE_ACCELERO
Wire.beginTransmission(addrNunchuck);// transmit to device 0x52
Wire.send(0x40);// sends memory address
Wire.send(0x00);// sends a zero, requesting the first data
Wire.endTransmission();// stop transmitting
delay(100);
#endif
#ifdef USE_BARO
baro.begin();
#endif
#ifdef USE_COMPASS
pidSetpoint = 0;
myPID.SetInputLimits (-180.0, 180.0);
myPID.SetOutputLimits(-100.0, 100.0);
myPID.SetMode(AUTO);
#endif
}
void loop()
{
...
#ifdef USE_COMPASS
Wire.beginTransmission(addrCompas);
Wire.send('A'); // command sensor to measure angle
Wire.endTransmission();
// step 2: wait for readings to happen
delay(10); // datasheet suggests at least 6000 microseconds
// step 3: request reading from sensor
Wire.requestFrom(addrCompas, static_cast<uint8_t>(2)); // request 2 bytes from slave device #33
// step 4: receive reading from sensor
int compassHeading = 0;
if(Wire.available() >= 2) // if two bytes were received
{
compassHeading = Wire.receive(); // receive high byte (overwrites previous reading)
compassHeading = compassHeading << 8; // shift high byte to be high 8 bits
compassHeading += Wire.receive(); // receive low byte as lower 8 bits
compassHeading /= 10;
compassHeading -= 90; // mounting position on the board
while(compassHeading < 0)
compassHeading += 360;
while(compassHeading > 360)
compassHeading -= 360;
if(doSendSerial)
Serial << "CH" << _DEC(compassHeading);
}
#endif
while(Serial.available())
evalSerialCmd();
#ifdef USE_DRIVEMOTOR
if(motorSpeed < 10)
{
analogWrite(pinMotorLower, 0);
analogWrite(pinMotorUpper, 0);
}
else
{
#ifndef USE_COMPASS
analogWrite(pinMotorLower, motorSpeed);
analogWrite(pinMotorUpper, motorSpeed);
#else // USE_COMPASS
// calculate the speed for the two rotors with heading correction
int headingDeviation = targetHeading - compassHeading;
while(headingDeviation < -180)
headingDeviation += 360;
while(headingDeviation > 180)
headingDeviation -= 360;
pidInput = headingDeviation;
myPID.Compute();
const int throttleDeviation = pidOutput - 50;
int throttleLower = motorSpeed - throttleDeviation;
throttleLower = max(throttleLower, 0);
throttleLower = min(throttleLower, 255);
int throttleUpper = motorSpeed + throttleDeviation;
throttleUpper = max(throttleUpper, 0);
throttleUpper = min(throttleUpper, 255);
analogWrite(pinMotorLower, throttleLower);
analogWrite(pinMotorUpper, throttleUpper);
if(doSendSerial)
Serial << "ML" << _DEC(throttleLower)
<< "MU" << _DEC(throttleUpper)
<< "HD" << _DEC(headingDeviation)
<< "TD" << _DEC(throttleDeviation);
#endif // USE_COMPASS
}
#endif // USE_DRIVEMOTOR
}
void evalCmdStr(char *txt)
{
if(!strcmp(txt, "lon"))
doSendSerial = true;
else if(!strcmp(txt, "loff"))
doSendSerial = false;
#ifdef USE_DRIVEMOTOR
else if(!strncmp(txt, "th", 2))
targetHeading = atoi(txt + 2);
else if(!strncmp(txt, "ms", 2))
motorSpeed = atoi(txt + 2);
else if(!strncmp(txt, "fb", 2))
srvFrontBack.write(atoi(txt + 2));
else if(!strncmp(txt, "lr", 2))
srvLeftRight.write(atoi(txt + 2));
#endif
#ifdef USE_RANGE_SCANNER
else if(!strcmp(txt, "swon"))
doSweepServo = true;
else if(!strcmp(txt, "swoff"))
{
doSweepServo = false;
rangescn.moveToIdlePos();
}
else if(!strncmp(txt, "swst", 4))
rangescn.setStepAngle(atoi(txt + 4));
else if(!strncmp(txt, "swrg", 4))
rangescn.setScanningRange(atoi(txt + 4));
#endif
else if(!strncmp(txt, "lode", 4))
{
int newval = atoi(txt + 4);
if(newval >= 10 && newval <= 1000)
loopDelay = newval;
}
else
{
#ifdef USE_HELP_MSG
prog_uchar helpstr[] PROGMEM =
"\nIrServoRangeScanner\n"
"'lon' send serial data\n"
"'loff' stop serial data\n"
"'swon' moving ir proximity sensor\n"
"'swoff' stop moving ir proximity sensor\n"
"'swst<nn>' angle deg per step\n"
"'swrg<nn>' scanning range\n"
"'th<nn>' desired compass heading\n"
"'ms<nn>' motor speed for the rotors\n"
"'fb<nn>' servo forward backward tilt\n"
"'lr<nn>' servo left right tilt\n"
"'lode<nn>' millisecs to sleep on loop\n"
"'?' for this help msg\n\n"
;
prog_uchar *cptr = helpstr;
char cc;
while(cc = pgm_read_byte(cptr++))
Serial.print(cc, BYTE);
#endif
if(strlen(txt) && strcmp(txt, "?"))
{
Serial.print("Invalid option: \"");
Serial.print(txt);
Serial.println("\"");
}
}
}