forked from SIAMatERAU/Eco-Dolphin
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Thruster.cpp
46 lines (39 loc) · 938 Bytes
/
Thruster.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
#include "Thruster.h"
#include <Servo.h>
Servo motor1;
Servo motor2;
Servo motor3;
Servo motor4;
int Thruster_Init()
{
//Serial.println("0");
motor1.attach(MOTOR1_PIN);
motor2.attach(MOTOR2_PIN);
motor3.attach(MOTOR3_PIN);
motor4.attach(MOTOR4_PIN);
motor1.writeMicroseconds(MID_SIGNAL);
motor2.writeMicroseconds(MID_SIGNAL);
motor3.writeMicroseconds(MID_SIGNAL);
motor4.writeMicroseconds(MID_SIGNAL);
delay(10000);
return 1;
}
void Thruster_Speed(int *TH)
{
motor1.writeMicroseconds(TH[0]);
// Serial.println("1");
motor2.writeMicroseconds(TH[1]);
// Serial.println("2");
motor3.writeMicroseconds(TH[2]);
// Serial.println("3");
motor4.writeMicroseconds(TH[3]);
// Serial.println("4");
delay(TH[4]);
}
void Thruster_Stop()
{
motor1.writeMicroseconds(MID_SIGNAL);
motor2.writeMicroseconds(MID_SIGNAL);
motor3.writeMicroseconds(MID_SIGNAL);
motor4.writeMicroseconds(MID_SIGNAL);
}