Navio+ RPI3 B PWM strong twitches (servos)

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;
}

Posts: 1

Participants: 1

Read full topic