void ControlPrepareNewGoal(void) {
control.order_started = 0;
PIDReset(&PID_left);
PIDReset(&PID_right);
}
/**
* Run all tests
*/
int
umain (void) {
isActivated = false;
isForward = true;
MagnetInit();
PIDInit();
uint8_t num_low_readings = 0;
activateRun();
while(1){
//Read Magnet data
ComputeMagnetTarget();
//uint8_t oldForward = isForward;
if (fangle > 90 || fangle < -90) {
isForward = false;
} else {
isForward = true;
}
//Determine if we should change from activated->deactivated or vice versa
if (isActivated) {
if (FieldStrength < DEACTIVATE_THRESH) {
num_low_readings++;
//Need START_RAMPDOWN readings to begin motor rampdown
if (num_low_readings > START_RAMPDOWN) {
SetTargetVelocity(TARGET_VELOCITY*(1.0-(float)(num_low_readings-START_RAMPDOWN)/(float)(DEACTIVATE_SAMPLES-START_RAMPDOWN)));
//SetTargetVelocity(TARGET_VELOCITY);
}
//If you have DEACTIVATE_SAMPLES low readings, deactivate
if (num_low_readings == DEACTIVATE_SAMPLES) {
isActivated = false;
num_low_readings = 0;
SetTargetVelocity(TARGET_VELOCITY);
printf("\nDEACTIVATED");
}
} else {
//We had a good reading, reset bad reading count and motor velocity
num_low_readings = 0;
SetTargetVelocity(TARGET_VELOCITY);
}
} else {
//Activate on a single high reading
if (FieldStrength > ACTIVATE_THRESH) {
isActivated = true;
SetTargetVelocity(TARGET_VELOCITY);
printf("\nACTIVATED");
PIDReset();
}
}
SetTargetVelocity(NearbyScale*TargetVelocity);
//Stop if not activated, otherwise update PID
if (!isActivated) {
CoastMotors();
//pause(100);
} else {
PIDUpdate();
}
//pause(100);
}
return 0;
}
