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();
}
}
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());
}
void EnhancedShooter::stopAll() {
stopWheel();
stopFeeder();
stopLift();
}
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();
}
}
