-
Notifications
You must be signed in to change notification settings - Fork 1
/
InMoov_Servo_Calibration.ino
123 lines (107 loc) · 3.97 KB
/
InMoov_Servo_Calibration.ino
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
/* ================================================================= //
// Ain Shams University - Faculty of Engineering //
// Ain Shams Virtal Hospital //
// HCM - Human Centered Mechatronics Lab. //
// InMoov Robot Project //
// //
// This is a unique work for InMoov Robot //
// at Human Centered Mechatronics (HCM) Lab - Ain Shams University //
// by: //
// ☑ Aly Mostafa Hafez //
// ☑ Hossam Nasr Elghareeb //
// //
// Date: August 2022 (Latest version) //
// //
// Hardware used: //
// · Arduino Mega //
// · 2 PCA9685 I2C Servo Drivers //
// · Servomotors (to be calibrated) //
// · HC-05 Bluetooth Module //
// ================================================================ */
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <SoftwareSerial.h>
//SoftwareSerial MyBlue(17, 16); // RX | TX
/* Insert here the pin number of the required servo to be calibrated
via Right I2C Servo driver */
int Right_pin_number = 0;
/* Insert here the pin number of the required servo to be calibrated
via Left I2C Servo driver */
int Left_pin_number = 0;
int flag = 2 ;
int counter = 300 ;
int DutyToDegree = 0 ;
char z = 0 ;
Adafruit_PWMServoDriver RightI2C = Adafruit_PWMServoDriver(0x40);
Adafruit_PWMServoDriver LeftI2C = Adafruit_PWMServoDriver(0x41);
#define SERVOMIN 150
#define SERVOMAX 600
#define USMIN 600
#define USMAX 2400
#define SERVO_FREQ 50
uint8_t servonum = 0;
void setup() {
Serial.begin(9600);
// MyBlue.begin(9600);
Serial.println("8 channel Servo test!");
RightI2C.begin();
LeftI2C.begin();
RightI2C.setOscillatorFrequency(27000000);
RightI2C.setPWMFreq(SERVO_FREQ);
delay(10);
LeftI2C.setOscillatorFrequency(27000000);
LeftI2C.setPWMFreq(SERVO_FREQ);
LeftI2C.setPWM(0, 0, counter);
RightI2C.setPWM(0, 0, counter);
delay(10);
}
void setServoPulse(uint8_t n, double pulse) {
double pulselength;
pulselength = 1000000; // 1,000,000 us per second
pulselength /= SERVO_FREQ;
Serial.print(pulselength); Serial.println(" us per period");
pulselength /= 4096; // 12 bits of resolution
Serial.print(pulselength); Serial.println(" us per bit");
pulse *= 1000000; // convert input seconds to us
pulse /= pulselength;
Serial.println(pulse);
RightI2C.setPWM(n, 0, pulse);
LeftI2C.setPWM(n, 0, pulse);
}
void loop()
{
//Blue() ;
if (Serial.available()) {
z = Serial.read();
// ==================== Right I2C ==================== //
if (z == 'a') {
if (counter < SERVOMAX) {
counter += 5;
RightI2C.setPWM(Right_pin_number, 0, counter);
}
}
else if (z == 'b') {
if (counter > SERVOMIN) {
counter -= 5;
RightI2C.setPWM(Right_pin_number, 0, counter);
}
}
// ==================== Left I2C ==================== //
else if (z == 'c') {
if (counter < SERVOMAX) {
counter += 5;
LeftI2C.setPWM(Left_pin_number, 0, counter);
}
}
else if (z == 'd') {
if (counter > SERVOMIN) {
counter -= 5;
LeftI2C.setPWM(Left_pin_number, 0, counter);
}
}
else {
z = 'z';
}
}
Serial.println(counter);
}