void SetNewConfigurationDataTable(uint8_t pin, int32_t value) {
println_E("Loading to datatable ");
p_int_E(pin);
print_E(" to ");
p_int_E(value);
if (down[pin].currentConfiguration != value) {
down[pin].changeConfiguration = true;
down[pin].currentConfiguration = value;
}
}
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 SetCoProcMode(uint8_t pin, uint8_t mode) {
if (GetChannelMode(pin) == mode)
return true;
println_E("Setting Mode: ");print_E(" on: ");p_int_E(pin);printMode(mode,ERROR_PRINT);
//getBcsIoDataTable(pin)->PIN.currentChannelMode = mode;
SetChannelModeDataTable(pin,mode);
down[pin].changeMode = true;
return false;
}
BYTE GetChannelMode(BYTE chan){
if(chan<0 || chan>GetNumberOfIOChannels()){
setPrintLevelErrorPrint();
#if !defined(__AVR_ATmega644P__) && !defined(__AVR_ATmega644PA__) && !defined(__AVR_ATmega324P__)
println_E("Failed IO sanity check: channel number out of bounds # ");p_int_E(chan);
#endif
//FAIL sanity check
while(1);
}
//Strip off the internally stored High Bit
return getBcsIoDataTable()[chan].PIN.currentChannelMode ;
}
BOOL initFlashLocal(){
if(bytesOfRaw > 0x1000-FLASHSTORE){
println_E("Too much data to store");
SoftReset();
}
println_W("Size of Flash data = ");p_int_W(bytesOfRaw);
SetFlashData( localData.data ,bytesOfRaw/4);
FlashLoad();
int i=0,j=0, index;
BOOL rawFlashDetect=FALSE;
for(i=0;i<numPidTotal;i++){
index = i*sizeof(AbsPID_Config);
for(j=0;j<sizeof(AbsPID_Config)/4;j++){
getPidGroupDataTable()[i].raw[j]=localData.data[index+j];
}
if( (getPidGroupDataTable()[i].config.Enabled != 1 &&
getPidGroupDataTable()[i].config.Enabled != 0) ){
rawFlashDetect = TRUE;
println_E("Detected raw flash, setting defaults : ");p_int_E(i);
printPIDvals(i);
getPidGroupDataTable()[i].config.Enabled = FALSE;
getPidGroupDataTable()[i].config.Async=0;
getPidGroupDataTable()[i].config.IndexLatchValue=0;
getPidGroupDataTable()[i].config.stopOnIndex=0;
getPidGroupDataTable()[i].config.useIndexLatch=0;
getPidGroupDataTable()[i].config.K.P=.1;
getPidGroupDataTable()[i].config.K.I=0;
getPidGroupDataTable()[i].config.K.D=0;
getPidGroupDataTable()[i].config.V.P=.1;
getPidGroupDataTable()[i].config.V.D=0;
getPidGroupDataTable()[i].config.Polarity=1;
getPidGroupDataTable()[i].config.stop=0;
getPidGroupDataTable()[i].config.upperHistoresis=0;
getPidGroupDataTable()[i].config.lowerHistoresis=0;
}
}
if(rawFlashDetect )
writeFlashLocal();
return !rawFlashDetect;
}
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 InitilizeBcsIo(int numPins,
DATA_STRUCT * dataPtrLocal,
BOOL (*setChanelValueHWPtrLocal)(BYTE,BYTE,INT32 *,float),
BOOL (*getChanelValueHWPtrLocal)(BYTE,BYTE*,INT32 *),
BOOL (*setAllChanelValueHWPtrLocal)(INT32 *,float),
BOOL (*getAllChanelValueHWPtrLocal)(INT32 *),
BOOL (*configChannelHWPtrLocal)(BYTE,BYTE,INT32 *)
){
if(numPins < 1
){
setPrintLevelErrorPrint();
println_E("Failed IO sanity check: failed initialization channels #");p_int_E(numPins);
//println_E("Failed IO sanity check: failed initialization channels #");p_int_E(numPins);
//FAIL sanity check
while(1);
}
if(
dataPtrLocal==NULL
){
setPrintLevelErrorPrint();
#if !defined(__AVR_ATmega644P__) && !defined(__AVR_ATmega644PA__) && !defined(__AVR_ATmega324P__)
println_E("Failed IO sanity check: failed initialization dataPtrLocal");
#endif
//println_E("Failed IO sanity check: failed initialization dataPtrLocal");
//FAIL sanity check
while(1);
}
if(
setChanelValueHWPtrLocal==NULL
){
setPrintLevelErrorPrint();
#if !defined(__AVR_ATmega644P__) && !defined(__AVR_ATmega644PA__) && !defined(__AVR_ATmega324P__)
println_E("Failed IO sanity check: failed initialization setChanelValueHWPtrLocal");
#endif
//println_E("Failed IO sanity check: failed initialization setChanelValueHWPtrLocal");
//FAIL sanity check
while(1);
}
if(
getChanelValueHWPtrLocal==NULL
){
setPrintLevelErrorPrint();
#if !defined(__AVR_ATmega644P__) && !defined(__AVR_ATmega644PA__) && !defined(__AVR_ATmega324P__)
println_E("Failed IO sanity check: failed initialization getChanelValueHWPtrLocal");
#endif
//println_E("Failed IO sanity check: failed initialization getChanelValueHWPtrLocal");
//FAIL sanity check
while(1);
}
if(
setAllChanelValueHWPtrLocal==NULL
){
setPrintLevelErrorPrint();
#if !defined(__AVR_ATmega644P__) && !defined(__AVR_ATmega644PA__) && !defined(__AVR_ATmega324P__)
println_E("Failed IO sanity check: failed initialization setAllChanelValueHWPtrLocal");
#endif
//println_E("Failed IO sanity check: failed initialization setAllChanelValueHWPtrLocal");
//FAIL sanity check
while(1);
}
if(
getAllChanelValueHWPtrLocal==NULL
){
setPrintLevelErrorPrint();
#if !defined(__AVR_ATmega644P__) && !defined(__AVR_ATmega644PA__) && !defined(__AVR_ATmega324P__)
println_E("Failed IO sanity check: failed initialization getAllChanelValueHWPtrLocal");
#endif
//println_E("Failed IO sanity check: failed initialization getAllChanelValueHWPtrLocal");
//FAIL sanity check
while(1);
}
if(
configChannelHWPtrLocal==NULL
){
setPrintLevelErrorPrint();
#if !defined(__AVR_ATmega644P__) && !defined(__AVR_ATmega644PA__) && !defined(__AVR_ATmega324P__)
println_E("Failed IO sanity check: failed initialization configChannelHWPtrLocal");
#endif
//println_E("Failed IO sanity check: failed initialization configChannelHWPtrLocal");
//FAIL sanity check
while(1);
}
NumberOfIOChannels = numPins;
dataPtr = dataPtrLocal;
setChanelValueHWPtr=setChanelValueHWPtrLocal;
getChanelValueHWPtr=getChanelValueHWPtrLocal;
setAllChanelValueHWPtr=setAllChanelValueHWPtrLocal;
getAllChanelValueHWPtr=getAllChanelValueHWPtrLocal;
configChannelHWPtr=configChannelHWPtrLocal;
}
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;
}
}
