Hi,
after several hours of trying to fix this I need some help.
I am trying to build a Tank with a robot-arm. This includes at least 6 Servos and 2 ESC.
If I plug the Servos in port 5,6,7, 9,10,11 and change channel 5 to 1540 (or more) , or change channel 9 to 1540 (or more) some Servos begin to randomly move in all directions (the arm). The keep doing this after I stop my programm. If I set the Servo back to 1500 they stop moving.
What did I miss ?
Thanks in Advance for any help.
I also tried to get a external PCA9685 over i2c to work, but without success (didn’t manage it to get in running properly yet).
System:
pi@navio:~ $ uname -a
Linux navio 4.14.34-emlid-v7+ #1 SMP PREEMPT Fri May 25 15:53:43 MSK 2018 armv7l GNU/Linux
Code:
//============================================================================
// Name : Tank.cpp
//============================================================================
#include <cstdlib>
#include <string>
#include <cstring>
#include <cctype>
#include <thread>
#include <chrono>
#include <unistd.h>
#include "Navio/Navio+/PCA9685.h"
#include "Navio/Common/gpio.h"
#include "Navio/Common/Util.h"
#include "Navio/Common/Ublox.h"
#include "Navio/Common/MPU9250.h"
#include "json/json.hpp"
#include <unistd.h>
#include <memory>
#include <algorithm>
#include <iostream>
#include <mqtt/async_client.h>
namespace {
const std::string TOPIC_PWM = "/tank/pwm";
}
using namespace Navio;
using namespace std;
using json = nlohmann::json;
#define SERVO_MIN 1250 /*mS*/
#define SERVO_MAX 1750 /*mS*/
#define SERVO_DEFAULT 1500 /*mS*/
#define SERVO_FREQUENCY 50
class user_callback: public virtual mqtt::callback {
void connection_lost(const std::string& cause) override {
std::cout << "\nConnection lost" << std::endl;
if (!cause.empty())
std::cout << "\tcause: " << cause << std::endl;
}
void delivery_complete(mqtt::delivery_token_ptr tok) override {
std::cout << "\n\t[Delivery complete for token: " << (tok ? tok->get_message_id() : -1) << "]" << std::endl;
}
public:
};
int main(int argc, char *argv[]) {
if (check_apm()) {
cout << "check_apm failed " << endl;
return 1;
}
mqtt::async_client client("tcp://192.168.1.1:1883", "TankPi");
cout << "Client created " << endl;
user_callback cb;
client.set_callback(cb);
mqtt::connect_options connOpts;
connOpts.set_keep_alive_interval(20);
connOpts.set_clean_session(true);
client.connect(connOpts)->wait();
cout << "Client connected " << endl;
client.start_consuming();
cout << "Client started consuming " << endl;
client.subscribe(TOPIC_PWM, 0)->wait();
cout << "Client subscribed " << endl;
static const uint8_t outputEnablePin = RPI_GPIO_27;
Pin pin(outputEnablePin);
if (pin.init()) {
pin.setMode(Pin::GpioModeOutput);
pin.write(0); /* drive Output Enable low */
}
PCA9685 pwm(0x40); //externes 0x40 0x41
if (!pwm.testConnection()) {
cout << "PWM connection unsucessful " << endl;
return 1;
}
pwm.setFrequency(SERVO_FREQUENCY);
pwm.setAllPWMuS(SERVO_DEFAULT);
pwm.initialize();
pwm.setFrequency(SERVO_FREQUENCY);
pwm.setAllPWMuS(SERVO_DEFAULT);
cout << "PWM initialized " << endl;
// This vector is used to store location data, decoded from ubx messages.
// After you decode at least one message successfully, the information is stored in vector
// in a way described in function decodeMessage(vector<double>& data) of class UBXParser(see ublox.h)
std::vector<double> pos_data;
// create ublox class instance
Ublox gps;
if (!gps.testConnection()) {
cout << "gps.testConnection()" << endl;
return 1;
}
cout << "GPS connected " << endl;
MPU9250 mpu;
cout << "mpu created " << endl;
mpu.initialize();
cout << "mpu initialized " << endl;
float ax, ay, az;
float gx, gy, gz;
float mx, my, mz;
json gpsJson;
json mpuJson;
float pwmState[16];
for (int i = 3; i < 16; i++) {
pwmState[i] = SERVO_DEFAULT;
}
cout << "Start loop " << endl;
while (true) {
mpu.update();
mpu.read_accelerometer(&ax, &ay, &az);
mpu.read_gyroscope(&gx, &gy, &gz);
mpu.read_magnetometer(&mx, &my, &mz);
mpuJson["ax"] = ax;
mpuJson["ay"] = ay;
mpuJson["az"] = az;
mpuJson["gx"] = gx;
mpuJson["gy"] = gy;
mpuJson["gz"] = gz;
mpuJson["mx"] = mx;
mpuJson["my"] = my;
mpuJson["mz"] = mz;
client.publish(mqtt::make_message("/tank/mpu", mpuJson.dump()));
if (gps.decodeSingleMessage(Ublox::NAV_POSLLH, pos_data) == 1) {
gpsJson["time"] = pos_data[0] / 1000; //"GPS Millisecond Time of Week: %.0lf s\n
gpsJson["longitude"] = pos_data[1] / 10000000;
gpsJson["latitude"] = pos_data[2] / 10000000;
gpsJson["height_ellipsoid"] = pos_data[3] / 1000;
gpsJson["height_sealevel"] = pos_data[4] / 1000;
gpsJson["horizontal_accuracy"] = pos_data[5] / 1000;
gpsJson["vertival_accuracy"] = pos_data[6] / 1000;
client.publish(mqtt::make_message("/tank/gps", gpsJson.dump()));
}
mqtt::const_message_ptr msg;
if (client.try_consume_message(&msg)) {
if (TOPIC_PWM.compare(msg->get_topic()) == 0) {
cout << "got on topic: " << msg->get_topic() << " message: " << msg->to_string() << endl;
auto pwmJson = json::parse(msg->to_string());
for (json::iterator it = pwmJson.begin(); it != pwmJson.end(); ++it) {
auto value = it.value();
int channel = stoi(it.key())+3;
int pwmValue = max(min(SERVO_MAX, (int) value), SERVO_MIN);
pwmState[channel] = pwmValue;
// pwm.setPWMuS(channel, pwmValue); // 1st Navio RC output is 3
cout << "pwm channel " << channel << " ms: " << pwmValue << endl;
}
} else {
cout << "got on topic: " << msg->get_topic() << " message: " << msg->to_string() << endl;
}
}
//Try to set the pulse every loop against twitches
for (int i = 3; i < 16; i++) {
pwm.setPWMuS(i, pwmState[i]); // 1st Navio RC output is 3
}
//todo different loop-times
usleep(500000);
}
return 0;
}