CAL_STATE pidHysterisis(int group) {
if (RunEvery(&getPidGroupDataTable(group)->timer) > 0) {
Print_Level l = getPrintLevel();
//setPrintLevelInfoPrint();
float boundVal = 150.0;
float extr = GetPIDPosition(group);
if (bound(0, extr, boundVal, boundVal)) {// check to see if the encoder has moved
//we have not moved
// println_I("NOT moved ");p_fl_I(extr);
if (getPidGroupDataTable(group)->calibration.state == forward) {
incrementHistoresis(group);
} else if (getPidGroupDataTable(group)->calibration.state == backward) {
decrementHistoresis(group);
}
int historesisBound = 25;
if (getPidGroupDataTable(group)->config.lowerHistoresis < (-historesisBound) &&
getPidGroupDataTable(group)->calibration.state == backward) {
println_E("Backward Motor seems damaged, more then counts of historesis #");
p_int_I(group);
getPidGroupDataTable(group)->calibration.state = forward;
}
if (getPidGroupDataTable(group)->config.upperHistoresis > (historesisBound) &&
getPidGroupDataTable(group)->calibration.state == forward) {
println_E("Forward Motor seems damaged, more then counts of historesis #");
p_int_I(group);
getPidGroupDataTable(group)->calibration.state = done;
}
} else {
pidReset(group, 0);
setOutput(group, 0);
println_E("Moved ");
p_fl_E(extr);
if (getPidGroupDataTable(group)->calibration.state == forward) {
println_I("Backward Calibrated for link# ");
p_int_I(group);
getPidGroupDataTable(group)->calibration.state = backward;
} else {
println_I("Calibration done for link# ");
p_int_I(group);
getPidGroupDataTable(group)->calibration.state = done;
float offset = .9;
getPidGroupDataTable(group)->config.lowerHistoresis *= offset;
getPidGroupDataTable(group)->config.upperHistoresis *= offset;
calcCenter(group);
}
}
if (getPidGroupDataTable(group)->calibration.state == forward) {
setOutput(group, 1.0f);
} else if (getPidGroupDataTable(group)->calibration.state == backward) {
setOutput(group, -1.0f);
}
setPrintLevel(l);
}
if (getPidGroupDataTable(group)->calibration.state == done)
SetPIDCalibrateionState(group, CALIBRARTION_DONE);
return getPidGroupDataTable(group)->calibration.state;
}
void cartesianAsync(){
if(RunEvery(&pid)){
int tmp =FifoGetPacketSpaceAvailible(&packetFifo);
if(tmp!= lastPushedBufferSize){
lastPushedBufferSize=tmp;
pushBufferEmpty();
full = FALSE;
}
checkPositionChange();
}
}
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();
}
}
void bowlerSystem(){
Bowler_Server_Local(&MyPacket);
float diff = RunEvery(&pid);
if(diff>0){
RunNamespaceAsync(&MyPacket,&asyncCallback);
if(diff>pid.setPoint){
println_E("Time diff ran over! ");p_fl_E(diff);
pid.MsTime=getMs();
}
cartesianAsync();
}
}
void startAdvancedAsyncDefault(uint8_t pin){
//println_W("Starting advanced async on channel: ");p_int_W(pin);
//int mode =GetChannelMode(pin);
//getBcsIoDataTable(pin)->PIN.asyncDataPreviousVal=0xffffffff;;
int32_t mode =GetChannelMode(pin);
if(mode == IS_ANALOG_IN){
float val = GetChanVal( pin) ;
configAdvancedAsyncDeadBand(pin,val*1.1,val*0.9);
}else{
getBcsIoDataTable(pin)->asyncDataTimer.MsTime=getMs();
getBcsIoDataTable(pin)->asyncDataTimer.setPoint=10;
getBcsIoDataTable(pin)->PIN.asyncDataType = NOTEQUAL;
}
RunEvery(getPinsScheduler( pin));
}
void UserRun(void){
#if defined(DEBUG)
if(GetChannelMode(16)!=IS_UART_TX){
setMode(16,IS_UART_TX);
ConfigureUART(115200);
}
#endif
#if defined(WPIRBE)
SPISlaveServer();
#endif
#if defined(USE_AS_LIBRARY)
RunUserCode();
#endif
// if (Get_UART_Byte_CountPassThrough()>0){
// PushSerial();
// }
if (RunEvery(&block0)>0.0f){
//println_W("Loop ");p_fl_W(getMs()/1000);
}
}
boolean pushAsyncReady( uint8_t pin){
if(!IsAsync(pin)){
//println_I("No asyinc on pin ");p_int_I(pin);print_I(" Mode 0x");prHEX8(GetChannelMode(pin),INFO_PRINT);
return false;
}
int32_t last;
int32_t aval;
int32_t db;
//int i=pin;
//EndCritical();
// println_I("Checking timer \nMsTime: ");p_fl_I(tRef->MsTime);
// print_I(" \nSetpoint: ");p_fl_I(tRef->setPoint);
// print_I(" \nCurrentTime: ");p_fl_I(getMs());
float timeout = RunEvery(getPinsScheduler( pin));
// print_I(" \nTimeout: ");p_fl_I(timeout);
if(GetChannelMode(pin)== IS_SERVO){
aval = GetServoPos(pin);
}else{
aval = getDataTableCurrentValue(pin);
}
if(timeout !=0){
// println_I("Time to do something");
switch(getBcsIoDataTable(pin)->PIN.asyncDataType){
case AUTOSAMP:
// println_I("Auto samp ");p_int_I(pin);
getBcsIoDataTable(pin)->PIN.asyncDataPreviousVal = aval;
return true;
case NOTEQUAL:
//
if(aval != getBcsIoDataTable(pin)->PIN.asyncDataPreviousVal){
//println_I("not equ ");p_int_I(pin);
//print_I("\t");
//p_int_I(getBcsIoDataTable(pin)->PIN.asyncDataPreviousVal);
//print_I("\t");
//p_int_I(getBcsIoDataTable(pin)->PIN.currentValue);
getBcsIoDataTable(pin)->PIN.asyncDataPreviousVal = aval;
return true;
}
break;
case DEADBAND:
last = getBcsIoDataTable(pin)->PIN.asyncDataPreviousVal;
db = getBcsIoDataTable(pin)->PIN.asyncDatadeadBandval;
if(!bound(last,aval,db,db)){
//println_I("deadband");p_int_I(pin);
getBcsIoDataTable(pin)->PIN.asyncDataPreviousVal=aval;
return true;
}
break;
case THRESHHOLD:
//println_I("treshhold");p_int_I(pin);
last = getBcsIoDataTable(pin)->PIN.asyncDataPreviousVal;
db = getBcsIoDataTable(pin)->PIN.asyncDatathreshholdval;
if(getBcsIoDataTable(pin)->PIN.asyncDatathreshholdedge == ASYN_RISING || getBcsIoDataTable(pin)->PIN.asyncDatathreshholdedge == ASYN_BOTH){
if(last<= db && aval>db){
getBcsIoDataTable(pin)->PIN.asyncDataPreviousVal = aval;
return true;
}
}
if(getBcsIoDataTable(pin)->PIN.asyncDatathreshholdedge == ASYN_FALLING|| getBcsIoDataTable(pin)->PIN.asyncDatathreshholdedge == ASYN_BOTH){
if(last> db && aval<=db){
getBcsIoDataTable(pin)->PIN.asyncDataPreviousVal = aval;
return true;
}
}
break;
default:
println_E("\nNo type defined!! chan: ");p_int_E(pin);
print_E(" mode: ");printMode(pin,GetChannelMode(pin),ERROR_PRINT);
print_E(" type: ");printAsyncType(pin,ERROR_PRINT);
startAdvancedAsyncDefault(pin);
break;
}
}else{
// println_I("Nothing to do, returning");
}
return false;
}
void checkLinkHomingStatus(int group) {
if (!(GetPIDCalibrateionState(group) == CALIBRARTION_home_down ||
GetPIDCalibrateionState(group) == CALIBRARTION_home_up ||
GetPIDCalibrateionState(group) == CALIBRARTION_home_velocity
)
) {
return; //Calibration is not running
}
float current = GetPIDPosition(group);
float currentTime = getMs();
if (RunEvery(&getPidGroupDataTable(group)->timer) > 0) {
//println_W("Check Homing ");
if (GetPIDCalibrateionState(group) != CALIBRARTION_home_velocity) {
float boundVal = getPidGroupDataTable(group)->homing.homingStallBound;
if (bound(getPidGroupDataTable(group)->homing.previousValue,
current,
boundVal,
boundVal
)
) {
pidReset(group, getPidGroupDataTable(group)->homing.homedValue);
//after reset the current value will have changed
current = GetPIDPosition(group);
getPidGroupDataTable(group)->config.tipsScale = 1;
println_W("Homing Velocity for group ");
p_int_W(group);
print_W(", Resetting position to: ");
p_fl_W(getPidGroupDataTable(group)->homing.homedValue);
print_W(" current ");
p_fl_W(current);
float speed = -20.0;
if (GetPIDCalibrateionState(group) == CALIBRARTION_home_up)
speed *= 1.0;
else if (GetPIDCalibrateionState(group) == CALIBRARTION_home_down)
speed *= -1.0;
else {
println_E("Invalid homing type");
return;
}
Print_Level l = getPrintLevel();
//setPrintLevelInfoPrint();
setOutput(group, speed);
setPrintLevel(l);
getPidGroupDataTable(group)->timer.MsTime = getMs();
getPidGroupDataTable(group)->timer.setPoint = 2000;
SetPIDCalibrateionState(group, CALIBRARTION_home_velocity);
getPidGroupDataTable(group)->homing.lastTime = currentTime;
}
} else {
current = GetPIDPosition(group);
float posDiff = current - getPidGroupDataTable(group)->homing.previousValue; //ticks
float timeDiff = (currentTime - getPidGroupDataTable(group)->homing.lastTime) / 1000.0; //
float tps = (posDiff / timeDiff);
getPidGroupDataTable(group)->config.tipsScale = 20 / tps;
println_E("New scale factor: ");
p_fl_E(getPidGroupDataTable(group)->config.tipsScale);
print_E(" speed ");
p_fl_E(tps);
print_E(" on ");
p_int_E(group);
print_E(" Position difference ");
p_fl_E(posDiff);
print_E(" time difference ");
p_fl_E(timeDiff);
OnPidConfigure(group);
SetPIDCalibrateionState(group, CALIBRARTION_DONE);
}
getPidGroupDataTable(group)->homing.previousValue = current;
}
}
