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); } }