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

Hello.

I am trying to build a Tank with a robot-arm.

It looks like you have an interesting project.

This includes at least 6 Servos and 2 ESC.

I think for your purposes it’d be better to use ArduRover since it has most necessary. You can set up more pwm output using SERVO_FUNCTION parameter.
Also ROS can be useful in such project.

The keep doing this after I stop my programm.

It happens because PCA output value, which you set and continue do it even if you stop updating it. So you need to reset it to TRIM value (1500) after program stop.

Thanks for the fast answer, but my original question is still without an answer.

Why would the servo on channel 10 (set to 1500) spin around randomly (without input at all) if I set channel 9 to 1540us ?

I already have a Setup for my Tank (used Arduino before) and like to repalce this with Navio because of the GPS,IMU and a Camera with live-video.

Update:
I managed to get the External PCA9685 up and running. The problem does not occur there.

I also checked the output of Navio+ PCA with an Ozilloscope and it looked fine. Can’t explain this continous random movement.
Maybe because the Servos are running at 7.4V but i got a working solution.

Thanks anyway :wink: