6 motor vecoterd thrust help

#1

Hi,

does anyone out there have any Arduino code for a 6 motor (4 forward/reverse & 2 up/down) ROV in vectored configuration. This is the code I have so far but im not sure if im on the right lines. Any help would be much appreciated.

#include <Servo.h>

int motorFwdPwmPin = { 12, 8, 4, 2, 6}; //pins 0-13 OK for PWM on Mega
int motorRevPwmPin = { 13, 9, 5, 3, 7}; //pins 0-13 OK for PWM on Mega

// vector calculation factors

float Vd = 0; //desired speed
float theta = 0; //desired angle
float Vtwist = 0; // rotation speed

void setup()
{

   // initialize motor pins

for (int x=0; x<numMotors; x++) {
pinMode (motorFwdPwmPin, OUTPUT);
pinMode (motorRevPwmPin, OUTPUT);
}

void runVectThrusters() { // calculate four thrusters speeds using vectors
if (!armAuto) return;
int motorSpeedWk;
float vMotor = {0,0,0,0};
vMotor[0] = Vd * sin(theta + PI/4) + Vtwist;
vMotor[1] = Vd * cos(theta + PI/4) - Vtwist;
vMotor[2] = Vd * cos(theta + PI/4) + Vtwist;
vMotor[3] = Vd * sin(theta + PI/4) - Vtwist;

 //run the motors 
  for (int x=0; x<4; x++) {
    if (vMotor[x] < -1) vMotor[x] = -100; //normalize
    else if (vMotor[x] > 1) vMotor[x] = 100;
    else vMotor[x] = vMotor[x] * 100;
    motorSpeedWk = map(abs(vMotor[x]), 0, 100, 0, 255);
    if (vMotor[x] < 0) {  //set motor direction
      analogWrite(motorFwdPwmPin[x], motorSpeedWk); //make it go
    } else {
      analogWrite(motorRevPwmPin[x], motorSpeedWk); //make it go
    }    
  }
0 Likes

#2

Hi Paul, you might want to have a look at the codebase of the Ardusub.com project since this already solves the problem. From there you should be able to derive the code you need to write for your arduino.

0 Likes