@@ -154,7 +154,7 @@ boolean uArmClass::set_servo_status(boolean setAttached, byte servoNum){
154154 double angleBefore = 90 ;
155155 if (servoNum == SERVO_ROT_NUM ) {
156156 if (setAttached) {
157- if (is_linear_calibrated == true ) {
157+ if (is_linear_calibrated == true && analogRead ( SERVO_ROT_ANALOG_PIN ) > 50 ) {
158158 angleBefore = read_servo_angle (SERVO_ROT_NUM );
159159 // Serial.println("New angle" + String(angleBefore));
160160 uarm.g_servo_rot .attach (SERVO_ROT_PIN );
@@ -170,7 +170,7 @@ boolean uArmClass::set_servo_status(boolean setAttached, byte servoNum){
170170 }
171171 }else if (servoNum == SERVO_LEFT_NUM ) {
172172 if (setAttached) {
173- if (is_linear_calibrated == true ) {
173+ if (is_linear_calibrated == true && analogRead ( SERVO_LEFT_ANALOG_PIN ) > 50 ) {
174174 angleBefore = uarm.read_servo_angle (SERVO_LEFT_NUM );
175175 uarm.g_servo_left .attach (SERVO_LEFT_PIN );
176176 uarm.g_servo_left .write (angleBefore);
@@ -185,7 +185,7 @@ boolean uArmClass::set_servo_status(boolean setAttached, byte servoNum){
185185 }
186186 }else if (servoNum == SERVO_RIGHT_NUM ) {
187187 if (setAttached) {
188- if (is_linear_calibrated == true ) {
188+ if (is_linear_calibrated == true && analogRead ( SERVO_RIGHT_ANALOG_PIN ) > 50 ) {
189189 angleBefore = uarm.read_servo_angle (SERVO_RIGHT_NUM );
190190 uarm.g_servo_right .attach (SERVO_RIGHT_PIN );
191191 uarm.g_servo_right .write (angleBefore);
@@ -200,7 +200,7 @@ boolean uArmClass::set_servo_status(boolean setAttached, byte servoNum){
200200 }
201201 }else if (servoNum == SERVO_HAND_ROT_NUM ) {
202202 if (setAttached) {
203- if (is_linear_calibrated == true ) {
203+ if (is_linear_calibrated == true && analogRead ( SERVO_HAND_ROT_ANALOG_PIN ) > 100 ) {
204204 angleBefore = uarm.read_servo_angle (SERVO_HAND_ROT_NUM );
205205 uarm.g_servo_hand_rot .attach (SERVO_HAND_PIN );
206206 uarm.g_servo_hand_rot .write (angleBefore);
0 commit comments