コード例 #1
0
void EnhancedShooter::update() {
    if(*robotState == robot_class::NORMAL)
    {
        if(liftTargetSet)
        {
            if(atPot(liftTarget))
            {
                liftTargetSet = false;
                stopLift();
            }
            else if(lift.GetPosition() < liftTarget)
            {
                lift.Set(0.3141592654);
            }
            else
            {
                lift.Set(-1.0f * 0.3141592654);
            }
        }
        if(HalEffect.Get() > 0)
        {
            stopFeeder();
            HalEffect.Reset();
            HalEffect.Start();
        }
        if(gunner -> GetRawButton(GUNNER_BTN_SHOOTER_WHEEL_REV)) {
            wheelForward = false;
            setSpeed(-1.0f * WHEEL_POWER);
        }
        else if(wheelForward)
        {
            setSpeed(WHEEL_POWER);
        }
        else
        {
            stopWheel();
        }
    }
    else
    {
        setLiftPower(-1.0f * LIFT_POWER); //Limit Switch Will Stop It
        stopFeeder();
        stopWheel();
    }
}
コード例 #2
0
ファイル: fizzy_smart_motor.cpp プロジェクト: hanalin/Fizzy
void FizzySmartMotor::controlTurning(int16_t degree,
                                     IFizzyEncoder* forward_motor_encoder,
                                     IFizzyEncoder* backward_motor_encoder) {

    int target_count = degree2Click(degree);
    int odo_count;
    bool turning = true;

    forward_motor_encoder->resetOdometer();
    backward_motor_encoder->resetOdometer();

    Serial.print("Forward: ");
    Serial.println(forward_motor_encoder->controlMotor());
    Serial.print("Backward: ");
    Serial.println(backward_motor_encoder->controlMotor());

    forwardWheel(forward_motor_encoder->controlMotor());
    backwardWheel(backward_motor_encoder->controlMotor());

    Serial.print("ODO Count Begin: ");

    while (turning) {

        turning = false;

        for (int i = 0; i < 2; i++) {

            odo_count = encoders[i]->odometerCount();
            Serial.print(odo_count);
            Serial.print(" ");

            if (odo_count >= target_count) {
                if (FIZZY_STATE_MOTOR_STOP != getState(encoders[i]->controlMotor())) {
                    stopWheel(encoders[i]->controlMotor());
                }
            }
            else {
                if (odo_count + 9 > target_count) {
                    // smooth stopping
                    brakeWheel(22 - (target_count - odo_count) * 2,
                               encoders[i]->controlMotor());
                }
                turning = true;
            }
        }
        Serial.print(",");
    }

    Serial.print("ODO Count End: ");
    Serial.println(encoders[0]->odometerCount());

}
コード例 #3
0
void EnhancedShooter::stopAll() {
    stopWheel();
    stopFeeder();
    stopLift();
}
コード例 #4
0
void loop(void) {
    loopCount++;

    if (digitalRead(BTN_PIN) == LOW) {
        leftCounter = 0;
        rightCounter = 0;
        delay(1000);
        driveWheel(WHEEL_LEFT, 150);
        driveWheel(WHEEL_RIGHT, 150);
        printDelay = millis();
    }

    tmpTime = micros();
    if ((leftReadedTime + ANALOG_READ_DELAY < tmpTime)
        || (leftReadedTime > tmpTime)
    ) {
        tmpValue = analogRead(LEFT_WHEEL_ENCODER_PIN);
        tmpIndex = leftReadedCount % ANALOG_READ_BUFFER_COUNT;
        leftReaded[tmpIndex] = tmpValue;

        tmpValue += leftReaded[(ANALOG_READ_BUFFER_COUNT + tmpIndex - 1) % ANALOG_READ_BUFFER_COUNT];
        tmpValue += leftReaded[(ANALOG_READ_BUFFER_COUNT + tmpIndex - 2) % ANALOG_READ_BUFFER_COUNT];

        if (tmpValue <= LOW_VALUE_THRESHOLD) {
            if (leftEncoderState) {
                leftEncoderState = 0;
            }
        } else if (tmpValue >= HIGH_VALUE_THRESHOLD) {
            if (!leftEncoderState) {
                leftEncoderState = 1;
                leftCounter++;
            }
        }

        leftReadedCount++;
        leftReadedTime = tmpTime;
    }

    tmpTime = micros();
    if ((rightReadedTime + ANALOG_READ_DELAY < tmpTime)
        || (rightReadedTime > tmpTime)
    ) {
        tmpValue = analogRead(RIGHT_WHEEL_ENCODER_PIN);
        tmpIndex = rightReadedCount % ANALOG_READ_BUFFER_COUNT;
        rightReaded[tmpIndex] = tmpValue;

        tmpValue += rightReaded[(ANALOG_READ_BUFFER_COUNT + tmpIndex - 1) % ANALOG_READ_BUFFER_COUNT];
        tmpValue += rightReaded[(ANALOG_READ_BUFFER_COUNT + tmpIndex - 2) % ANALOG_READ_BUFFER_COUNT];

        if (tmpValue <= LOW_VALUE_THRESHOLD) {
            if (rightEncoderState) {
                rightEncoderState = 0;
            }
        } else if (tmpValue >= HIGH_VALUE_THRESHOLD) {
            if (!rightEncoderState) {
                rightEncoderState = 1;
                rightCounter++;
            }
        }

        rightReadedCount++;
        rightReadedTime = tmpTime;
    }

    tmpTime = micros();
    if ((sensorReadedTime + SENSOR_READ_DELAY < tmpTime)
        || (leftReadedTime > tmpTime)
    ) {
        tmpIndex = (sensorReadedCount % SENSOR_READ_BUFFER_COUNT);
        for (int i = 0; i < LASER_SENSORS_COUNT; i++) {
            analogSensorValue[i][tmpIndex] = analogRead(laserSensors[i]);
        }
        sensorReadedCount++;
        sensorReadedTime = tmpTime;
    }

    if (printDelay + 1000 < millis()) {
        stopWheel(WHEEL_LEFT);
        stopWheel(WHEEL_RIGHT);

        Serial.print("left=");
        Serial.print(leftCounter);
        Serial.print(" right=");
        Serial.print(rightCounter);
        Serial.print(" loops=");
        Serial.print(loopCount);

        for (int i = 0; i < LASER_SENSORS_COUNT; i++) {
            Serial.print("\tS");
            Serial.print(i);
            Serial.print("=");
            Serial.print(getSensorValue(i));
        }
        Serial.println();

        loopCount = 0;
        printDelay = millis();
    }
}