Skip to content

Commit 5825d39

Browse files
altjzaltjz
committed
add calibration ino
1 parent 29275bf commit 5825d39

4 files changed

Lines changed: 683 additions & 424 deletions

File tree

‎HISTORY.md‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,15 @@
1+
## [1.5.7] - 2016-05-07
2+
3+
### Fix
4+
5+
- Update SERVO_OFFST_ADDRESS to MANUAL_OFFSET_ADDRESS
6+
- Update indent for syntax
7+
8+
### Changes
9+
10+
- Add Calibration.ino into examples
11+
12+
113
## [1.5.6] - 2016-05-06
214

315
### Fix

‎examples/Calibration/Calibration.ino‎

Lines changed: 247 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,247 @@
1+
/******************************************************************************************
2+
* File Name : Calibration.ino
3+
* Author : Joey Song
4+
* Update : Alex Tan
5+
* Version : V2.0
6+
* Description : This documents is for calibration with uArm Metal version
7+
* Copyright(C) 2016 uArm Team. All right reserved.
8+
*******************************************************************************************/
9+
10+
#include "uarm_library.h"
11+
12+
int value; // value is the data recevied
13+
14+
void setup() {
15+
16+
Serial.begin(9600); // start serial port at 9600 bps
17+
18+
}
19+
20+
21+
void loop() {
22+
23+
if(Serial.available()>0)
24+
{
25+
26+
char readSerial = Serial.read();
27+
Serial.println(readSerial);
28+
29+
// Input c to start calibrate automatically
30+
if (readSerial == 'c') {
31+
calibration_start();
32+
delay(1000);
33+
uarm.moveTo(0,-15,6);
34+
}
35+
36+
//---------------------------------- Test Function ------------------------------------
37+
if (readSerial == '1') {
38+
uarm.moveTo(15,-15,5);
39+
delay(1000);
40+
}
41+
42+
43+
if (readSerial == '2') {
44+
uarm.moveTo(-15,-15,5);
45+
delay(1000);
46+
}
47+
48+
if (readSerial == 's') {
49+
Serial.println("linear offset:");
50+
for(byte i = 0;i < 4; i++)
51+
{
52+
Serial.print("Linear Offset Servo ");
53+
Serial.println(i);
54+
double a = 0.0f;
55+
double b = 0.0f;
56+
uarm.readLinearOffset(i,a,b);
57+
Serial.print("A: ");
58+
Serial.print(a);
59+
Serial.print(", B: ");
60+
Serial.println(b);
61+
Serial.print("Manual Offset: ");
62+
Serial.print(uarm.readServoOffset(i));
63+
Serial.println("");
64+
}
65+
delay(1000);
66+
}
67+
}
68+
}
69+
70+
71+
/** Start Calibration all section
72+
**/
73+
void calibration_start(){
74+
75+
cleanEEPROM();
76+
77+
for (int k = 0; k < 4; k++) {
78+
79+
linear_calibration_servo(k);
80+
delay(2000);
81+
}
82+
EEPROM.write(CALIBRATION_LINEAR_FLAG, CONFIRM_FLAG);
83+
manual_calibration_section();
84+
EEPROM.write(CALIBRATION_MANUAL_FLAG, CONFIRM_FLAG);
85+
EEPROM.write(CALIBRATION_FLAG, CONFIRM_FLAG);
86+
87+
Serial.println("All done!");
88+
}
89+
90+
/** Calibrate each servo for linear offset
91+
**/
92+
void linear_calibration_servo(byte servo_num)
93+
{
94+
const byte kServoRangeIni = 20;
95+
const byte kServoRangeFin = 100;
96+
double l_angle_analog;
97+
double arr_real[16];
98+
double arr_input[16];
99+
int intercept_address = LINEAR_INTERCEPT_START_ADDRESS;
100+
int slope_address = LINEAR_SLOPE_START_ADDRESS;
101+
Serial.print("intercept_address: ");
102+
Serial.println(intercept_address);
103+
Serial.print("slope_address: ");
104+
Serial.println(slope_address);
105+
Serial.print("servo ");
106+
Serial.println(servo_num);
107+
108+
byte l_analog_pin;
109+
110+
for (byte i = 0; i < (((kServoRangeFin-kServoRangeIni)/5)+1); i++)
111+
{
112+
byte dot_i = 5*i;
113+
float angle = kServoRangeIni+dot_i;
114+
switch(servo_num)
115+
{
116+
case SERVO_ROT_NUM:
117+
l_analog_pin = SERVO_ROT_ANALOG_PIN;
118+
uarm.writeServoAngle(SERVO_ROT_NUM, angle, false);
119+
uarm.writeLeftRightServoAngle(60, 30, false);
120+
break;
121+
122+
case SERVO_LEFT_NUM:
123+
l_analog_pin = SERVO_LEFT_ANALOG_PIN;
124+
uarm.writeServoAngle(SERVO_ROT_NUM, 90, false);
125+
uarm.writeLeftRightServoAngle(angle, 30, false);
126+
break;
127+
128+
case SERVO_RIGHT_NUM:
129+
l_analog_pin = SERVO_RIGHT_ANALOG_PIN;
130+
uarm.writeServoAngle(SERVO_ROT_NUM, 90, false);
131+
uarm.writeLeftRightServoAngle(30, angle, false);
132+
break;
133+
134+
case SERVO_HAND_ROT_NUM:
135+
l_analog_pin = SERVO_HAND_ROT_ANALOG_PIN;
136+
uarm.writeServoAngle(SERVO_ROT_NUM, 90, false);
137+
uarm.writeLeftRightServoAngle(60, 30, false);
138+
uarm.writeServoAngle(SERVO_HAND_ROT_NUM, angle, false);
139+
break;
140+
default:
141+
142+
break;
143+
}
144+
145+
if(i == 0) {
146+
delay(2000);
147+
}
148+
149+
for (int l = 0; l<3; l++) {
150+
l_angle_analog = analogRead(l_analog_pin);
151+
delay(100);
152+
}
153+
154+
arr_real[i] = kServoRangeIni + dot_i;
155+
arr_input[i] = l_angle_analog;
156+
157+
delay(800);
158+
159+
}
160+
arr_real[0] = kServoRangeIni;
161+
162+
LinearRegression lr(arr_input, arr_real, 16);
163+
Serial.print("lr.getA():");
164+
Serial.println(lr.getA());
165+
Serial.print("lr.getB():");
166+
Serial.println(lr.getB());
167+
save_linear_servo_offset(servo_num, lr.getA(), lr.getB());
168+
}
169+
170+
/** Clean EEPROM before calibration
171+
**/
172+
void cleanEEPROM(){
173+
for(int p = 0; p<EEPROM.length(); p++) {
174+
EEPROM.write(p,0);
175+
}
176+
}
177+
178+
/** Manual Calibration Section
179+
**/
180+
void manual_calibration_section()
181+
{
182+
int setLoop = 1;
183+
184+
uarm.detachAll();
185+
186+
Serial.println("Put uarm in calibration posture (servo 1 to 3: 45, 130, 20 degree respectively), then input 1");
187+
while (setLoop) {
188+
189+
if (Serial.available()>0) {
190+
191+
char inputChar = Serial.read();
192+
193+
if (inputChar=='1')
194+
{
195+
double offsetRot = uarm.readAngle(SERVO_ROT_NUM,true) - 45;
196+
double offsetL = uarm.readAngle(SERVO_LEFT_NUM,true) - 130;
197+
double offsetR = uarm.readAngle(SERVO_RIGHT_NUM,true) - 20;
198+
199+
Serial.print("Offsets for servo 1 to 3 are ");
200+
Serial.println(offsetRot);
201+
Serial.println(offsetL);
202+
Serial.println(offsetR);
203+
204+
205+
// if (abs(offsetRot)>25.4||abs(offsetL)>25.4||abs(offsetR)>25.4)
206+
// {
207+
// Serial.print("Check posture");
208+
// }
209+
// else{
210+
save_manual_servo_offset(SERVO_ROT_NUM, offsetRot);
211+
save_manual_servo_offset(SERVO_LEFT_NUM, offsetL);
212+
save_manual_servo_offset(SERVO_RIGHT_NUM, offsetR);
213+
setLoop = 0;
214+
Serial.println("Save offset done! ");
215+
// }
216+
217+
218+
}
219+
else if(inputChar== 'e')
220+
{
221+
Serial.println("exit");
222+
setLoop = 0;
223+
}
224+
225+
else{
226+
Serial.println("Incorrect, input again, or e to exit");
227+
}
228+
229+
}
230+
231+
}
232+
}
233+
234+
/** Save Manual Servo Offset
235+
*
236+
**/
237+
void save_manual_servo_offset(byte servo_num, double offset)
238+
{
239+
EEPROM.put(MANUAL_OFFSET_ADDRESS + servo_num * sizeof(offset), offset);
240+
}
241+
242+
/** Save Linear Servo Offset intercept & slope
243+
**/
244+
void save_linear_servo_offset(byte servo_num, double intercept_val, double slope_val){
245+
EEPROM.put(LINEAR_INTERCEPT_START_ADDRESS + servo_num * sizeof(intercept_val), intercept_val);
246+
EEPROM.put(LINEAR_SLOPE_START_ADDRESS + servo_num * sizeof(slope_val), slope_val);
247+
}

0 commit comments

Comments
 (0)