Propulsion system and brain

Post date: Jul 10, 2015 6:1:31 AM

Principle:

There is 4 12V DC motor, 2 left step propeller and 2 right step propeller, the 2 left step on the left side one at the back on horizontal axis and one on the top on vertical axis, symmetric with the right side.

I'm used to fly my drone in Mode 2 remote control configuration so i'm going to use the same for the ROV control.

Thrust and yaw on left stick and pitch and roll on right stick (see pict above)

The mixer should be something like that:

Pitch=M1++ M2++ (forward) or M1-- M2-- (backward)

Thrust= M3++ M4++(Down) or M3-- M4-- (Up)

Yaw= M1++ M2-- (Right) or M1-- M2++ (Left)

Roll= M3++ M4-- (Right) or M3-- M4++ (Left)

Dc Motors and frame:

From Jonhson Bilge pump cartridge i extract the DC motor part by removing all cartridge an impeller.

Those DC motor are supposed to be waterproof (I hope to 5 bars).

Unfortunately, i did not find 4 bilge pump with the same rating and i ended up with 2 johnson pump 1650 and 2 johnson pump 1450.

The 1450 is obviously less power full than the 1650. I did some consumption measurement with a lipo 12.5V battery directly connected on the DC motor via an ampermeter.

Johnson 1650 use 1A (1.2A at start)

Johnson 1450 use 750Ma (1A at start)

Total consumption for the 4 DC motors full speed 3.5A (4.4A at start)

I put the 1450 (less power) on the vertical axis and the 1650 (more power) on the horizontal axis.

The propellers are linked to the motor output with Graupner Shaft adapter and RC 3 blade boat propellers (2 left, 2 right)

For the frame and fixation a piece of PVC pipe and 2 PVC T junction, Hot glue, and few cable Tie did the job.

For the Remote controller i have extracted from a cheap USB Joy Stick the board with the 2 stick and reversed engineered it to find where are connected the 4 potentiometers.

I sold it to a Prototype PCB board (very useful prototyping board from semageek),

I'm using a 4 pair ethernet cable to pass from the arduino to the remote controller board the following signal:

  • Pair 1: +V/Gnd (from arduino)

  • Pair 2-3 Pot1, Pot2, Pot3, Po4,

  • Pair 4: Video+/Gnd from the Gopro to female RCA.

Now it's time to sold everything on and do some coding.

Components:

Schematic:

Tether:

Code:

I wrote the code bellow to interconnect one direction on the joystick or one potentiometer to one DC motor.

Yaw is connected to M1

Roll is connected to M2

Pitch is connected to M3

Thrust is connected to M4

The direction is going to change according to the reading of the potentiometer value.

Pot value >512 Direction 1 Pot value <511 Direction 2.

Direction 1 is the direction of the propeller step

Direction 2 is reversed

Due to propeller step,

Direction for M1 and M2 are reversed

Direction for M3 and M4 are reversed

/* Connections:

* BOARD -> ARDUINO

* 1A -> 2

* 1B -> 4

* E1 -> 3

* 2A -> 7

* 2B -> 8

* E2 -> 5

* 3A -> 6

* 3B -> 10

* 3E -> 11

* 4A -> 13

* 4B -> 12

* 4E -> 9

* Yaw -> A0

* Pitch -> A3

* Roll -> A2

* Thrust -> A1

##############################################################################*/ // Define constants and variablesconst int DirM1a = 2;const int DirM1b = 4;const int Motor1 = 3;const int DirM2a = 7;const int DirM2b = 8;const int Motor2 = 5;const int DirM3a = 6;const int DirM3b = 10;const int Motor3 = 11;const int DirM4a = 13;const int DirM4b = 12;const int Motor4 = 9;const int Yaw = A0; const int Roll = A2; const int Pitch= A3; const int Thrust = A1; int analogValueYaw = 0; int analogValueRoll = 0; int analogValuePitch = 0;int analogValueThrust = 0;byte pwmMotor1 = 0;byte pwmMotor2 = 0;byte pwmMotor3 = 0;byte pwmMotor4 = 0;// Initializationvoid setup() { pinMode(DirM1a, OUTPUT); pinMode(DirM1b, OUTPUT); pinMode(Motor1, OUTPUT); pinMode(DirM2a, OUTPUT); pinMode(DirM2b, OUTPUT); pinMode(Motor2, OUTPUT); pinMode(DirM3a, OUTPUT); pinMode(DirM3b, OUTPUT); pinMode(Motor3, OUTPUT); pinMode(DirM4a, OUTPUT); pinMode(DirM4b, OUTPUT); pinMode(Motor4, OUTPUT); }// main loopvoid loop() { // read potentiometers values analogValueYaw = analogRead(Yaw); analogValueRoll = analogRead(Roll); analogValuePitch = analogRead(Pitch); analogValueThrust = analogRead(Thrust); // apply direction adjustment for channel 1 if(analogValueYaw > 512) { digitalWrite(DirM1a, HIGH); digitalWrite(DirM1b, LOW); pwmMotor1 = (analogValueYaw - 512)/2; // evaluate new pwm value } else { digitalWrite(DirM1a, LOW); digitalWrite(DirM1b, HIGH); pwmMotor1 = (511 - analogValueYaw)/2; // evaluate new pwm value } // apply direction adjustment for channel 2 if(analogValueRoll > 512) { digitalWrite(DirM2a, HIGH); digitalWrite(DirM2b, LOW); pwmMotor2 = (analogValueRoll - 512)/2; // evaluate new pwm value } else { digitalWrite(DirM2a, LOW); digitalWrite(DirM2b, HIGH); pwmMotor2 = (511 - analogValueRoll)/2; // evaluate new pwm value }// apply direction adjustment for channel 3 if(analogValuePitch > 512) { digitalWrite(DirM3a, HIGH); digitalWrite(DirM3b, LOW); pwmMotor3 = (analogValuePitch - 512)/2; // evaluate new pwm value } else { digitalWrite(DirM3a, LOW); digitalWrite(DirM3b, HIGH); pwmMotor3 = (511 - analogValuePitch)/2; // evaluate new pwm value } // apply direction adjustment for channel 4 if(analogValueThrust > 512) { digitalWrite(DirM4a, HIGH); digitalWrite(DirM4b, LOW); pwmMotor4 = (analogValueThrust - 512)/2; // evaluate new pwm value } else { digitalWrite(DirM4a, LOW); digitalWrite(DirM4b, HIGH); pwmMotor4 = (511 - analogValueThrust)/2; // evaluate new pwm value } // apply speed adjustment analogWrite(Motor1,pwmMotor1); analogWrite(Motor2,pwmMotor2); analogWrite(Motor3,pwmMotor3); analogWrite(Motor4,pwmMotor4);}

Conclusion:

Success!!!!!

Nothing had burned when plugged in, the 4 DC motor react as planned.

Consumption was as expected.

The Code helped me to check and identify position and rotation of the motors and the corresponding cabling to the arduino.

There is a little vibration on each DC motor when they are stopped.

The command signal from the arduino to the DC motor driver is a constant "LOW" but when i plug an oscilloscope at the Driver output i have a square peak from +15V to -15V of 100 μs followed by a fading 6 v fading sinusoidal for 200μs

I don't know if i have to worried about that. I'll do further investigation about that.

Oups!!!! I got it. The command signal from the arduino to the DC motor driver is not that flat but produce a small peak. It's because of the neutral zone of my pot. i assume in my code that it's 512 but it is not and the arduino is sending a pulse signal correspoding to the pot reading. The pulse lenght is to short to turn the motor but enought to make it vibrate. I should chose a range of neutral value fort he 4 potentiometers.

Video transmission:

The video signal from the gopro to my black pearl screen via the 10m tether is crystal clear. That;s a pretty good news because i have absolutely no idea of the maximum distance i can reach through a twisted pair cable :)

Now it's time to work on the DC Motor Mixer.

<<Previous step Project Description Next step DC motor mixer>>