miniCopterPro/effectorsWrapper.cpp at master · cb1986ster/miniCopterPro · GitHub

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
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
/**
	This file is for my 4 motor, 2-axis gimbal copter
*/
#include "effectorsWrapper.h"

#include "miniCopterPro.h"

void effectorsWrapper::stopMotors(){
	motorsSpeed[0] = 0;
	motorsSpeed[1] = 0;
	motorsSpeed[2] = 0;
	motorsSpeed[3] = 0;
};


void effectorsWrapper::init(){
	((miniCopterPro*)copterPointer)->io.sendMesgNoNl(ioText_effectorsInit);

	gimbalInit();
	motorInit();
#ifdef USE_SOFTWARE_SERVO_CONTROL
	SoftwareServo::refresh();
#endif

	/* All done */
	((miniCopterPro*)copterPointer)->io.sendMesgNoStart(ioText_OK);
}

void effectorsWrapper::update(){
	gimbalUpdate();
	motorsUpdate();
#ifdef USE_SOFTWARE_SERVO_CONTROL
	SoftwareServo::refresh();
#endif
}

void effectorsWrapper::gimbalInit(){
#ifdef USE_SOFTWARE_SERVO_CONTROL
	gimbal[0].attach(10);
	gimbal[1].attach(11);
#else
	gimbal[0].attach(10,670,1830);
	gimbal[1].attach(11,670,1830);
#endif
	setGimbalArc(0,0);
	setGimbalArc(1,0);
	gimbalUpdate();
}

void effectorsWrapper::gimbalUpdate(){
	gimbal[0].write(
		constrain((int)(gimbalArc[0]*71)+71,3,142)
	);
	gimbal[1].write(
		constrain((int)(gimbalArc[1]*71)+71,3,142)
	);
}

void effectorsWrapper::motorInit(){
#ifdef USE_SOFTWARE_SERVO_CONTROL
	motor[0].attach(9);
	motor[1].attach(6);
	motor[2].attach(5);
	motor[3].attach(3);
#else
	motor[0].attach(9,700,2000);
	motor[1].attach(6,700,2000);
	motor[2].attach(5,700,2000);
	motor[3].attach(3,700,2000);
#endif
	setMotorSpeed(0,0);
	setMotorSpeed(1,0);
	setMotorSpeed(2,0);
	setMotorSpeed(3,0);
	motorsUpdate();
}

void effectorsWrapper::motorsUpdate(){
	motor[0].write(
		constrain((int)(motorsSpeed[0]*180.0),EFFECTORS_MOTOR_MIN_VALUE,EFFECTORS_MOTOR_MAX_VALUE)
	);
	motor[1].write(
		constrain((int)(motorsSpeed[1]*180.0),EFFECTORS_MOTOR_MIN_VALUE,EFFECTORS_MOTOR_MAX_VALUE)
	);
	motor[2].write(
		constrain((int)(motorsSpeed[2]*180.0),EFFECTORS_MOTOR_MIN_VALUE,EFFECTORS_MOTOR_MAX_VALUE)
	);
	motor[3].write(
		constrain((int)(motorsSpeed[3]*180.0),EFFECTORS_MOTOR_MIN_VALUE,EFFECTORS_MOTOR_MAX_VALUE)
	);
}