void moveRobotSpeed ( unsigned int direction, unsigned int speed )
{
int speedLeftServo = 0;
int speedRightServo = 0;
switch ( direction )
{
case FORWARD:
speedLeftServo = speed;
speedRightServo = -speed;
break;
case REVERSE:
speedLeftServo = -speed;
speedRightServo = speed;
break;
case LEFT:
speedLeftServo = -speed;
speedRightServo = -speed;
break;
case RIGHT:
speedLeftServo = speed;
speedRightServo = speed;
break;
case HALT:
speedLeftServo = 0;
speedRightServo = 0;
break;
}
setServoSpeed(0, speedLeftServo);
setServoSpeed(1, speedRightServo);
}
void UF_uArm::init()
{
// read offset data
offsetL = EEPROM.read(1);
offsetR = EEPROM.read(2);
// initialization the pin
pinMode(LIMIT_SW, INPUT); digitalWrite(LIMIT_SW, HIGH);
pinMode(BTN_D4, INPUT); digitalWrite(BTN_D4, HIGH);
pinMode(BTN_D7, INPUT); digitalWrite(BTN_D7, HIGH);
pinMode(BUZZER, OUTPUT); digitalWrite(BUZZER, LOW);
pinMode(PUMP_EN, OUTPUT); digitalWrite(PUMP_EN, LOW);
pinMode(VALVE_EN, OUTPUT); digitalWrite(VALVE_EN, LOW);
if (EEPROM.read(0) == CALIBRATION_FLAG) // read of offset flag
{
// attaches the servo on pin to the servo object
servoL.attach(SERVO_L, D150A_SERVO_MIN_PUL, D150A_SERVO_MAX_PUL);
servoR.attach(SERVO_R, D150A_SERVO_MIN_PUL, D150A_SERVO_MAX_PUL);
servoRot.attach(SERVO_ROT, D150A_SERVO_MIN_PUL, D150A_SERVO_MAX_PUL);
servoHand.attach(SERVO_HAND, D009A_SERVO_MIN_PUL, D009A_SERVO_MAX_PUL);
servoHandRot.attach(SERVO_HAND_ROT, D009A_SERVO_MIN_PUL, D009A_SERVO_MAX_PUL);
servoHand.write(HAND_ANGLE_OPEN, 0, true);
servoHand.detach();
servoL.write(map(readAngle(SERVO_L), SERVO_MIN, SERVO_MAX, 0, 180));
servoR.write(map(readAngle(SERVO_R), SERVO_MIN, SERVO_MAX, 0, 180));
servoRot.write(map(readAngle(SERVO_ROT), SERVO_MIN, SERVO_MAX, 0, 180));
// initialization postion
setServoSpeed(SERVO_R, 20); // 0=full speed, 1-255 slower to faster
setServoSpeed(SERVO_L, 20); // 0=full speed, 1-255 slower to faster
setServoSpeed(SERVO_ROT, 20); // 0=full speed, 1-255 slower to faster
setPosition(stretch, height, rotation, handRot);
}
else
{ // buzzer alert if calibration needed
alert(3, 200, 200);
}
}
