Hexapod_lib/Servo.cpp at main · Franword/Hexapod_lib · 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
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
#include "Servo.h"
#include <Arduino.h>
Servo::Servo(){};
void Servo::Setup(uint8_t leg_num,uint8_t servo_num)
{
	switch (leg_num) {
		case R1:
			_pwm_num=false;
			_inverse=false;
			//Serial.print ("R1 ");
			if (servo_num==q1){
				_address=R1q1;}
			if (servo_num==q2){
				_address=R1q2;}
			if (servo_num==q3){
				_address=R1q3;}
		break;
		case R2:
			_pwm_num=true;
			_inverse=false;
			//Serial.print ("R2 ");
			if (servo_num==q1){
				_address=R2q1;}
			if (servo_num==q2){
				_address=R2q2;}
			if (servo_num==q3){
				_address=R2q3;}
		break;
		case R3:
			_pwm_num=true;
			_inverse=false;
			//Serial.print ("R3 ");
			if (servo_num==q1){
				_address=R3q1;}
			if (servo_num==q2){
				_address=R3q2;}
			if (servo_num==q3){
				_address=R3q3;}
		break;
		case L1:
			_pwm_num=false;
			_inverse=true;
			//Serial.print ("L1 ");
			if (servo_num==q1){
				_address=L1q1;}
			if (servo_num==q2){
				_address=L1q2;}
			if (servo_num==q3){
				_address=L1q3;}
		break;
		case L2:
			_pwm_num=false;
			_inverse=true;
			//Serial.print ("L2 ");
			if (servo_num==q1){
				_address=L2q1;}
			if (servo_num==q2){
				_address=L2q2;}
			if (servo_num==q3){
				_address=L2q3;}
		break;
		case L3:
			_pwm_num=true;
			_inverse=true;
			//Serial.print ("L3 ");
			if (servo_num==q1){
				_address=L3q1;}
			if (servo_num==q2){
				_address=L3q2;}
			if (servo_num==q3){
				_address=L3q3;}
	}
	_id=3*leg_num+servo_num;

	ReadOffset();
	//SetServoAngle(90,false);
};
uint16_t Servo::SetServoAngle(float angle,bool relative)
{
	//Serial.println("\nServo::SetServoAngle");
	if(relative){
		if(_inverse){
			angle=_angle-angle;
		}
		else{
			angle=_angle+angle;
		}
	}
	else{
		if(_inverse){
			angle=180.0-angle-float(_offset);
		}
		else{
			angle=angle+float(_offset);
		}
	}
	if(angle<0){
		Serial.print("angle=");
		Serial.print(angle);
		Serial.print(", Servo_num= q");
		Serial.println(_id%3+1);
		angle=90;
	}
	if(angle>180){
		Serial.print("angle=");
		Serial.print(angle);
		Serial.print(", Servo_num= q");
		Serial.println(_id%3+1);
		angle=90;
	}
	_angle=angle;
	return map_float(angle, 0, 180, SERVOMIN,SERVOMAX);
	//Serial.println(_pulse);
};
void Servo::info(bool if_current){
	if(if_current){
		if(_inverse){
			Serial.print(180-(_angle+_offset));
		}
		else{
			Serial.print(_angle-_offset);
		}
	}
	else{
		Serial.print(_offset);
	}
};
void Servo::SetOffset(){
	if(_inverse){ //inversing againg to set values using inversing xD
		EEPROM.update(_id,128+(180-int(_angle))-90);
	}
	else{
		EEPROM.update(_id,128+int(_angle)-90);
	}
};
void Servo::ReadOffset(){
	_offset =EEPROM.read(_id)-128;
	//Serial.print(_offset);
};
bool Servo::get_pwm_num(){
	return _pwm_num;
};
uint8_t  Servo::get_pwm_address(){
	return _address;
};
float Servo::get_angle(){
	if(_inverse){
			return 180.0-(_angle+float(_offset));
		}
		else{
			return _angle-float(_offset);
		}
};
int Servo::map_float(float x, int in_min, int in_max, int out_min, int out_max){
	return int((x - float(in_min)) * (float(out_max - out_min)) / (float(in_max - in_min)) + float(out_min));
};