void RunPDVel(uint8_t chan){
//println_I("Running PID vel");
if(getPidVelocityDataTable(chan)->enabled==true) {
getPidGroupDataTable(chan)->Output=runPdVelocityFromPointer(getPidVelocityDataTable(chan),
getPidGroupDataTable(chan)->CurrentState,
getPidGroupDataTable(chan)->config.V.P,
getPidGroupDataTable(chan)->config.V.D
);
if(GetPIDCalibrateionState(chan)<=CALIBRARTION_DONE)
setOutput(chan,getPidGroupDataTable(chan)->Output);
}
}
boolean processPIDCrit(BowlerPacket * Packet){
uint8_t i=0;
switch (Packet->use.head.RPC){
case KPID:
for(i=0;i<getNumberOfPidChannels();i++){
getPidGroupDataTable(i)->config.Enabled = true;
setOutput(i,0.0);
getPidGroupDataTable(i)->config.Enabled = false;
getPidVelocityDataTable(i)->enabled=false;
getPidGroupDataTable(i)->Output=0.0;
}
READY(Packet,zone,0);
break;
case CPID:
if(ConfigPID(Packet)){
READY(Packet,zone,1);
}else
ERR(Packet,zone,1);
break;
case CPDV:
if(ConfigPDVelovity(Packet)){
READY(Packet,zone,1);
}else
ERR(Packet,zone,1);
break;
default:
return false;
}
return true;
}
uint8_t SetPIDTimed(uint8_t chan, float val, float ms) {
//println_I("@#@# PID channel Set chan=");p_int_I(chan);print_I(" setpoint=");p_int_I(val);print_I(" time=");p_fl_I(ms);
if (chan >= getNumberOfPidChannels())
return false;
getPidVelocityDataTable(chan)->enabled = false;
return SetPIDTimedPointer(getPidGroupDataTable(chan), val, GetPIDPosition(chan), ms);
}
void StartPDVel(uint8_t chan,float unitsPerSeCond,float ms){
if(ms<.1){
//println_I("Starting Velocity");
getPidVelocityDataTable(chan)->enabled=true;
getPidGroupDataTable(chan)->config.Enabled=false;
getPidVelocityDataTable(chan)->lastPosition=GetPIDPosition(chan);
getPidVelocityDataTable(chan)->lastTime=getMs();
getPidVelocityDataTable(chan)->unitsPerSeCond=unitsPerSeCond;
getPidVelocityDataTable(chan)->currentOutputVel =0;
}else{
//println_I("Starting Velocity Timed");
float seConds = ms/1000;
int32_t dist = (int32_t) unitsPerSeCond*(int32_t) seConds;
int32_t delt = ((int32_t) (GetPIDPosition(chan))-dist);
SetPIDTimed(chan, delt, ms);
}
}
void pidReset(uint8_t chan, int32_t val) {
float value = pidResetNoStop(chan, val);
AbsPID * data = getPidGroupDataTable(chan);
data->interpolate.set = value;
data->interpolate.setTime = 0;
data->interpolate.start = value;
data->interpolate.startTime = getMs();
data->SetPoint = value;
uint8_t enabled = data->config.Enabled;
data->config.Enabled = true; //Ensures output enabled to stop motors
data->Output = 0.0;
setOutput(chan, data->Output);
getPidVelocityDataTable(chan)->enabled = enabled;
}
float pidResetNoStop(uint8_t chan, int32_t val) {
AbsPID * data = getPidGroupDataTable(chan);
//float value = (float)resetPosition(chan,val);
float current = data->CurrentState;
float raw = current + data->config.offset;
float value = (float) val;
data->config.offset = (raw - value);
data->CurrentState = raw - data->config.offset;
// println_E("From pidReset Current State: ");
// p_fl_E(current);
// print_E(" Target value: ");
// p_fl_E(value);
// print_E(" Offset: ");
// p_int_E(data->config.offset);
// print_E(" Raw: ");
// p_int_E(raw);
float time = getMs();
data->lastPushedValue = val;
InitAbsPIDWithPosition(getPidGroupDataTable(chan), data->config.K.P, data->config.K.I, data->config.K.D, time, val);
getPidVelocityDataTable(chan)->lastPosition = val;
getPidVelocityDataTable(chan)->lastTime = time;
return val;
}
