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;
}
void checkCalibration(int group) {
if (getPidGroupDataTable(group)->calibration.calibrated != true) {
getPidGroupDataTable(group)->config.upperHistoresis = 0;
getPidGroupDataTable(group)->config.lowerHistoresis = 0;
getPidGroupDataTable(group)->config.stop = 0;
getPidGroupDataTable(group)->calibration.calibrated = true;
}
}
boolean isPIDArrivedAtSetpoint(int index, float plusOrMinus) {
if (getPidGroupDataTable(index)->config.Enabled)
return bound(getPidGroupDataTable(index)->SetPoint,
getPidGroupDataTable(index)->CurrentState,
plusOrMinus,
plusOrMinus);
return true;
}
void GetConfigPDVelocity(BowlerPacket * Packet){
// 2700 4 8 2712 a98 output/o/PidRpc.o
set32bit(Packet,getPidGroupDataTable(Packet->use.data[0])->config.V.P*100, 1);
set32bit(Packet,getPidGroupDataTable(Packet->use.data[0])->config.V.D*100, 5);
Packet->use.head.DataLegnth=4+9;
Packet->use.head.Method=BOWLER_POST;
}
void initializeCartesianController(){
InitPacketFifo(&packetFifo,buffer,SIZE_OF_PACKET_BUFFER);
int i=0;
for(i=0;i<3;i++){
if(getPidGroupDataTable()[linkToHWIndex(i)].config.Enabled!=TRUE){
getPidGroupDataTable()[linkToHWIndex(i)].config.Enabled=TRUE;
getPidGroupDataTable()[linkToHWIndex(i)].config.Polarity=TRUE;
getPidGroupDataTable()[linkToHWIndex(i)].config.K.P=.12;
getPidGroupDataTable()[linkToHWIndex(i)].config.K.I=.4;
OnPidConfigure(linkToHWIndex(i));
}
}
}
int main(){
//setPrintLevelInfoPrint();
setPrintLevelWarningPrint();
//setPrintLevelNoPrint();
hardwareInit();
RunEveryData loop = {0.0,2000.0};
while(1){
if (_RF5==1){
setPrintLevelErrorPrint();
p_int_E(0);print_E(" Reset Button Pressed from loop");
SetColor(1,1,1);
U1CON = 0x0000;
DelayMs(100);
Reset();
}
if(RunEvery(&loop)>0){
// clearPrint();
// printCartesianData();
}
if( printCalibrations == false &&
GetPIDCalibrateionState(linkToHWIndex(0))==CALIBRARTION_DONE&&
GetPIDCalibrateionState(linkToHWIndex(1))==CALIBRARTION_DONE&&
GetPIDCalibrateionState(linkToHWIndex(2))==CALIBRARTION_DONE
){
printCalibrations = true;
int index=0;
for(index=0;index<3;index++){
int group = linkToHWIndex(index);
println_E("For Axis ");p_int_E(group);
print_E(" upper: ");p_int_E(getPidGroupDataTable(group)->config.upperHistoresis);
print_E(" lower: ");p_int_E(getPidGroupDataTable(group)->config.lowerHistoresis);
print_E(" stop: ");p_int_E(getPidGroupDataTable(group)->config.stop);
}
startHomingLinks();
//Print_Level l= getPrintLevel();
//setPrintLevelInfoPrint();
printCartesianData();
int i;
for(i=0;i<numPidMotors;i++){
printPIDvals(i);
}
//setPrintLevel(l);
}
bowlerSystem();
}
}
uint8_t ConfigPDVelovity(BowlerPacket * Packet){
uint8_t chan = Packet->use.data[0];
float KP=0;
float KD=0;
KP=(float)get32bit(Packet,1);
KD=(float)get32bit(Packet,5);
getPidGroupDataTable(chan)->config.V.P=KP/100.0;
getPidGroupDataTable(chan)->config.V.D=KD/100.0;
OnPidConfigure(chan);
return true;
}
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);
}
}
void InitilizePidController(AbsPID * groups, int numberOfGroups,
float (*getPositionPtr)(int),
void (*setOutputPtr)(int, float),
int (*resetPositionPtr)(int, int),
void (*onPidConfigurePtr)(int),
PidLimitEvent* (*checkPIDLimitEventsPtr)(uint8_t group)) {
if (groups == 0 ||
getPositionPtr == 0 ||
setOutputPtr == 0 ||
resetPositionPtr == 0 ||
checkPIDLimitEventsPtr == 0 ||
onPidConfigurePtr == 0) {
println("Null pointer exception in PID Configure", ERROR_PRINT);
while (1);
}
pidGroupsInternal =groups;
number_of_pid_groups = numberOfGroups;
getPosition = getPositionPtr;
setOutputLocal = setOutputPtr;
resetPosition = resetPositionPtr;
onPidConfigureLocal = onPidConfigurePtr;
checkPIDLimitEvents = checkPIDLimitEventsPtr;
SetControllerMath(&RunAbstractPIDCalc);
int i;
for (i = 0; i < numberOfGroups; i++) {
int enabled = getPidGroupDataTable(i)->config.Enabled;
pidReset(i, 0);
//getPidGroupDataTable(i)->config.Enabled = enabled;
SetPIDEnabled(i, enabled);
//println_I("PID ");p_int_I(i);print_I(" enabled");
}
}
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 RunVel(void){
uint8_t i;
for (i=0;i<getNumberOfPidChannels();i++){
//println_I("Checking velocity on ");p_int_I(i);
if(!getPidGroupDataTable(i)->config.Enabled){
RunPDVel(i);
}
}
}
void setOutput(int group, float val) {
if(bound(0,getPidGroupDataTable(group)->config.tipsScale, .001, .001)){
println_W("PID TPS Sclale close to zero");p_fl_W(getPidGroupDataTable(group)->config.tipsScale);
}
val *= getPidGroupDataTable(group)->config.tipsScale;
val += getPidStop(group);
if (val > getPidStop(group) && val < getUpperPidHistoresis(group) ){
val = getUpperPidHistoresis(group);
println_E("Upper histerisys");
}
if (val < getPidStop(group) && val > getLowerPidHistoresis(group)){
val = getLowerPidHistoresis(group);
println_E("Lower histerisys");
}
getPidGroupDataTable(group)->OutputSet = val;
//
setOutputLocal(group, val);
}
BOOL isCartesianInterpolationDone(){
updateCurrentPositions();
float targets[3] = {xCurrent,yCurrent,zCurrent};
int setpointBound = 200;
float mmPositionResolution = .3;
int i;
for(i=0;i<4;i++){
if(i<3){
if(!bound(intCartesian[i].set,targets[i],mmPositionResolution,mmPositionResolution)){
//println_W("Interpolation not done on to: ");p_fl_W(intCartesian[i].set);print_W(" is = ");p_fl_W(targets[i]);
return FALSE;
}
}
if( (isPIDArrivedAtSetpoint(linkToHWIndex(i), setpointBound) == FALSE) && (i==3) ){
println_W("LINK not done moving index = ");p_int_W(linkToHWIndex(i));
print_W(" currently is = "); p_fl_W(getPidGroupDataTable()[linkToHWIndex(i)].CurrentState);
print_W(" heading towards = "); p_fl_W(getPidGroupDataTable()[linkToHWIndex(i)].SetPoint);
return FALSE;
}
}
return TRUE;
}
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;
}
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 writeFlashLocal(){
if(bytesOfRaw > 0x1000-FLASHSTORE){
println_E("Too much data to store");
SoftReset();
}
println_W("Writing values to Flash");
int i=0,j=0, index;
for(i=0;i<numPidTotal;i++){
index = i*sizeof(AbsPID_Config);
for(j=0;j<sizeof(AbsPID_Config)/4;j++){
localData.data[index+j]=getPidGroupDataTable()[i].raw[j];
}
}
FlashSync();
FlashLoad();
for(i=0;i<numPidTotal;i++){
printPIDvals(i);
}
}
void runPidHysterisisCalibration(int group) {
if (!getPidGroupDataTable(group)->config.Enabled) {
println_E("Axis disabled for calibration #");
p_int_E(group);
getPidGroupDataTable(group)->config.Enabled = true;
}
getPidGroupDataTable(group)->config.lowerHistoresis = 0;
getPidGroupDataTable(group)->config.upperHistoresis = 0;
getPidGroupDataTable(group)->config.stop = 0;
// println_I("\tReset PID");
pidReset(group, 0); // Zero encoder reading
// println_I("\tDisable PID Output");
SetPIDEnabled(group, true);
SetPIDCalibrateionState(group, CALIBRARTION_hysteresis);
getPidGroupDataTable(group)->calibration.state = forward;
// println_I("\tSetting slow move");
setOutput(group, -1.0f);
getPidGroupDataTable(group)->timer.setPoint = 2000;
getPidGroupDataTable(group)->timer.MsTime = getMs();
}
void startHomingLink(int group, PidCalibrationType type, float homedValue) {
float speed = 20.0;
if (type == CALIBRARTION_home_up)
speed *= 1.0;
else if (type == CALIBRARTION_home_down)
speed *= -1.0;
else {
println_E("Invalid homing type");
return;
}
getPidGroupDataTable(group)->config.tipsScale = 1;
SetPIDCalibrateionState(group, type);
setOutput(group, speed);
getPidGroupDataTable(group)->timer.MsTime = getMs();
getPidGroupDataTable(group)->timer.setPoint = 1000;
getPidGroupDataTable(group)->homing.homingStallBound = 20;
getPidGroupDataTable(group)->homing.previousValue = GetPIDPosition(group);
getPidGroupDataTable(group)->homing.lastTime = getMs();
getPidGroupDataTable(group)->homing.homedValue = homedValue;
SetPIDEnabled(group,true);
}
void initPIDChans(uint8_t group){
if(dyPid[group].inputChannel==DYPID_NON_USED || dyPid[group].outputChannel==DYPID_NON_USED){
return;
}
switch(dyPid[group].inputMode){
case IS_COUNTER_INPUT_INT:
case IS_COUNTER_INPUT_DIR:
case IS_COUNTER_INPUT_HOME:
dyPid[group].inputChannel = getCounterIntChannnel( channelToCounterGroup(dyPid[group].inputChannel));
StartCounterInput(dyPid[group].inputChannel);
break;
}
// println_W("PID In chan: ");
// p_int_W(dyPid[group].inputChannel);
// println_W(" mode: ");
// printMode(dyPid[group].inputMode, WARN_PRINT);
// println_W("PID Out chan: ");
// p_int_W(dyPid[group].outputChannel);
// println_W(" mode: ");
//printMode(dyPid[group].outputMode, WARN_PRINT);
SetCoProcMode(dyPid[group].inputChannel,dyPid[group].inputMode);
SetCoProcMode(dyPid[group].outputChannel,dyPid[group].outputMode);
//SyncModes();
if(dyPid[group].inputMode== IS_ANALOG_IN){
pidGroups[group].SetPoint=GetValFromAsync(dyPid[group].inputChannel);
}else{
pidGroups[group].SetPoint=0;
}
SetPIDCalibrateionState(group, CALIBRARTION_DONE);
getPidGroupDataTable( group)->config.Async=true;
//getPidGroupDataTable( group)->config.Enabled=true;
}
void RunPIDControl() {
int i;
for (i = 0; i < getNumberOfPidChannels(); i++) {
//println_E(" PID Loop ");p_int_E(i);
getPidGroupDataTable(i)->CurrentState = getPosition(i) - getPidGroupDataTable(i)->config.offset;
if (getPidGroupDataTable(i)->config.Enabled == true) {
getPidGroupDataTable(i)->SetPoint = interpolate(getPidInterpolationDataTable(i), getMs());
MathCalculationPosition(getPidGroupDataTable(i), getMs());
if (GetPIDCalibrateionState(i) <= CALIBRARTION_DONE) {
setOutput(i, getPidGroupDataTable(i)->Output);
} else if (GetPIDCalibrateionState(i) == CALIBRARTION_hysteresis) {
pidHysterisis(i);
} else if ((GetPIDCalibrateionState(i) == CALIBRARTION_home_down) ||
(GetPIDCalibrateionState(i) == CALIBRARTION_home_up) ||
(GetPIDCalibrateionState(i) == CALIBRARTION_home_velocity)) {
checkLinkHomingStatus(i);
}
}
}
}
int GetPIDPosition(uint8_t chan) {
if (chan >= getNumberOfPidChannels())
return 0;
//getPidGroupDataTable(chan)->CurrentState=(int)getPosition(chan);
return getPidGroupDataTable(chan)->CurrentState;
}
uint8_t ConfigPID(BowlerPacket * Packet){
uint8_t chan = Packet->use.data[0];
// int i;
// println_W("Starting config");
// for(i=0;i<getNumberOfPidChannels();i++){
// printPIDvals(i);
// }
getPidGroupDataTable(chan)->config.Polarity = ((Packet->use.data[2]==0)?0:1);
getPidGroupDataTable(chan)->config.Async = ((Packet->use.data[3]==0)?0:1);
float KP=0;
float KI=0;
float KD=0;
float temp=0;
KP=(float)get32bit(Packet,4);
KI=(float)get32bit(Packet,8);
KD=(float)get32bit(Packet,12);
if(Packet->use.head.DataLegnth>(4+16)){
temp=(float)get32bit(Packet,16);
getPidGroupDataTable(chan)->config.useIndexLatch= Packet->use.data[20];
getPidGroupDataTable(chan)->config.stopOnIndex = Packet->use.data[21];
getPidGroupDataTable(chan)->config.stop=(float)get32bit(Packet,22)/1000.0;
getPidGroupDataTable(chan)->config.upperHistoresis=(float)get32bit(Packet,26)/1000.0;
getPidGroupDataTable(chan)->config.lowerHistoresis=(float)get32bit(Packet,30)/1000.0;
getPidGroupDataTable(chan)->config.calibrationState = CALIBRARTION_DONE;
}else{
temp=0;
getPidGroupDataTable(chan)->config.useIndexLatch= true;
getPidGroupDataTable(chan)->config.stopOnIndex = true;
getPidGroupDataTable(chan)->config.stop=0;
getPidGroupDataTable(chan)->config.upperHistoresis=0;
getPidGroupDataTable(chan)->config.lowerHistoresis=0;
}
getPidGroupDataTable(chan)->config.IndexLatchValue=(float)temp;
getPidGroupDataTable(chan)->config.K.P=KP/100;
getPidGroupDataTable(chan)->config.K.I=KI/100;
getPidGroupDataTable(chan)->config.K.D=KD/100;
//b_println("Resetting PID channel from Config:",INFO_PRINT);printBowlerPacketDEBUG(Packet,INFO_PRINT);
//b_println("From Config Current setpoint:",INFO_PRINT);p_fl(getPidGroupDataTable(chan)->SetPoint,INFO_PRINT);
OnPidConfigure(chan);
getPidGroupDataTable(chan)->config.Enabled = ((Packet->use.data[1]==0)?0:1);
return true;
}
void GetConfigPID(BowlerPacket * Packet){
uint8_t chan = Packet->use.data[0];
Packet->use.data[1]=getPidGroupDataTable(chan)->config.Enabled;// = ((Packet->use.data[1]==0)?0:1);
Packet->use.data[2]=getPidGroupDataTable(chan)->config.Polarity;// = ((Packet->use.data[2]==0)?0:1);
Packet->use.data[3]=getPidGroupDataTable(chan)->config.Async;//= ((Packet->use.data[3]==0)?0:1);
set32bit(Packet,getPidGroupDataTable(chan)->config.K.P*100,4);
set32bit(Packet,getPidGroupDataTable(chan)->config.K.I*100,8);
set32bit(Packet,getPidGroupDataTable(chan)->config.K.D*100,12);
set32bit(Packet,getPidGroupDataTable(chan)->config.IndexLatchValue,16);
//latching data
Packet->use.data[20]=getPidGroupDataTable(chan)->config.useIndexLatch;//
Packet->use.data[21]=getPidGroupDataTable(chan)->config.stopOnIndex;//
set32bit(Packet,getPidGroupDataTable(chan)->config.stop*1000,22);
set32bit(Packet,getPidGroupDataTable(chan)->config.upperHistoresis*1000,26);
set32bit(Packet,getPidGroupDataTable(chan)->config.lowerHistoresis*1000,30);
Packet->use.head.DataLegnth=4+22+(3*4);
Packet->use.head.Method=BOWLER_POST;
}
PidCalibrationType GetPIDCalibrateionState(int group) {
return getPidGroupDataTable(group)->config.calibrationState;
}
boolean isPIDInterpolating(int index) {
return getPidGroupDataTable(index)->interpolate.setTime != 0;
}
boolean isPidEnabled(uint8_t i) {
return getPidGroupDataTable(i)->config.Enabled;
}
void SetPIDEnabled(uint8_t index, boolean enabled) {
AbsPID * tmp = getPidGroupDataTable(index);
tmp->config.Enabled = enabled;
}
uint8_t ClearPID(uint8_t chan) {
if (chan >= getNumberOfPidChannels())
return false;
getPidGroupDataTable(chan)->config.Enabled = false;
return true;
}
void setPIDConstants(int group, float p, float i, float d) {
getPidGroupDataTable(group)->config.K.P = p;
getPidGroupDataTable(group)->config.K.I = i;
getPidGroupDataTable(group)->config.K.D = d;
}