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 writeWordFlash(uint32_t address,uint32_t data){
if (address >= StartAppVectPhysical && (address < EndAppVectPhysical)){
NVMWriteWord((uint32_t*)address, data);
if ((*(int *)(address|0x80000000)) != data){
println_E("FAULT read did not match write on address: ");prHEX32(address,ERROR_PRINT);
eraseFlash();
callBootloaderReset();
}
}else{
println_E("FAULT can not reach address: ");prHEX32(address,ERROR_PRINT);
}
}
INTERPOLATE_DATA * getPidInterpolationDataTable(int group) {
if (pidGroupsInternal == NULL|| group<0 || group >= number_of_pid_groups) {
println_E("Velocity data table is null");
while(1);
}
return &pidGroupsInternal[group].interpolate;
}
PD_VEL * getPidVelocityDataTable(int group) {
if (pidGroupsInternal == NULL|| group<0 || group >= number_of_pid_groups) {
println_E("Velocity data table is null");
while(1);
}
return &pidGroupsInternal[group].vel;
}
void updateServoValues(){
int32_t ip;
if(blockIndex>=dataTableSize){
println_E("Bad block size");
return;
}
// Interpolate position
ip =getInterpolatedPin(blockIndex);
if(GetChannelMode(blockIndex) == IS_SERVO)
blockData[blockIndex].positionTempA=ip;
else
blockData[blockIndex].positionTempA = 0;
// Interpolate position
ip =getInterpolatedPin(blockIndex +12);
if(GetChannelMode(blockIndex+12) == IS_SERVO)
blockData[blockIndex].positionTempB=ip;
else
blockData[blockIndex].positionTempB = 0;
}
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;
}
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;
}
AbsPID * getPidGroupDataTable(int group) {
if (pidGroupsInternal == NULL || group<0 || group >= number_of_pid_groups) {
println_E("PID data table is null");
while(1);
}
// Internal reference stores the address of the base of the array
// Add to that the size of the struct times the index. THis should create
// a pointer to the address of this specific array address
return &pidGroupsInternal[group];
}
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);
}
void runPIDConfigurationValueSync(){
int i;
for (i=0;i<NUM_PID_GROUPS;i++){
if(dyPid[i].flagValueSync == true){
dyPid[i].flagValueSync = false;
println_E("Syncing PID values");
WritePIDvalues(&pidGroups[i],&dyPid[i],i);
}
}
}
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;
}
}
void updateCurrentPositions(){
if(servostock_calcForward(
getLinkAngle(0),
getLinkAngle(1),
getLinkAngle(2),
&xCurrent,
&yCurrent,
&zCurrent)!=0){
println_E("Inverse Failed!!") ;
return;
}
}
BYTE setXYZ(float x, float y, float z,float ms){
updateCurrentPositions();
float t0=0,t1=0,t2=0;
if(hwMap.iK_callback( x, y, z, &t0, &t1, &t2)==0){
println_I("New target angles t1=");p_fl_I(t0);print_I(" t2=");p_fl_I(t1);print_I(" t3=");p_fl_I(t2);
setLinkAngle(0,t0,ms);
setLinkAngle(1,t1,ms);
setLinkAngle(2,t2,ms);
}else{
println_E("Interpolate failed, can't reach: x=");p_fl_E(x);print_E(" y=");p_fl_E(y);print_E(" z=");p_fl_E(z);
}
}
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 ;
}
DATA_STRUCT * getBcsIoDataTable(){
if(dataPtr==NULL){
setPrintLevelErrorPrint();
#if !defined(__AVR_ATmega644P__) && !defined(__AVR_ATmega644PA__) && !defined(__AVR_ATmega324P__)
println_E("Failed IO sanity check: no data table");
#endif
//println_E("Failed IO sanity check: no data table");
//FAIL sanity check
while(1);
}
return dataPtr;
}
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();
}
}
int servostock_calcInverse(float X, float Y, float Z, float *Alpha, float *Beta, float *Gamma){
float L = getRodLength();
float R = getBaseRadius()-getEndEffectorRadius();
float Lsqr=L*L;
float maxRad=sqrt((X*X)+(Y*Y));
//#warning "Z is not used yet"
if((maxRad>(L-R))|| (Z<getminZ())||(Z>(getmaxZ()+L))){
println_E("Outside of workspace x=");p_fl_E(X);print_E(" y=");p_fl_E(Y);print_E(" z=");p_fl_E(Z);print_E(" Bound radius=");p_fl_E((maxRad));
//printf("\r\nOutside of workspace x= %g y=%g z=%g Bound = %g",X,Y,Z,maxRad);
return 1;//This is ourside the reachable work area
}
float SIN_60 = 0.8660254037844386;
float COS_60 = 0.5;
// Values are in mm, Alpha, Beta, Gamma starts at 0 at the base platform.
Alpha[0] = sqrt(Lsqr - (0 - X)*(0 - X) - (R - Y)*(R - Y))+Z;
Beta[0] = sqrt(Lsqr - (-SIN_60*R - X)*(-SIN_60*R - X) - (-COS_60*R - Y)*(-COS_60*R - Y))+Z;
Gamma[0] = sqrt(Lsqr - (SIN_60*R - X)*(SIN_60*R - X) - (-COS_60*R - Y)*(-COS_60*R - Y))+Z;
if( abs(Alpha[0]-Beta[0])>L||
abs(Alpha[0]-Gamma[0])>L||
abs(Beta[0]-Alpha[0])>L||
abs(Beta[0]-Gamma[0])>L||
abs(Gamma[0]-Alpha[0])>L||
abs(Gamma[0]-Beta[0])>L){
println_E("Outside of workspace x=");p_fl_E(X);print_E(" y=");p_fl_E(Y);print_E(" z=");p_fl_E(Z);print_E(" Bound radius=");p_fl_E((maxRad));
println_E("Alpha=");p_fl_E(Alpha[0]);
print_E(" Beta=");p_fl_E(Beta[0]);
print_E(" Gama=");p_fl_E(Gamma[0]);
return 1;//This is ourside the reachable work area
}
return 0;//SUCCESS
}
uint8_t Bowler_Server_Local(BowlerPacket * Packet){
Print_Level l = getPrintLevel();
//setPrintLevelNoPrint();
if (GetBowlerPacket_arch(Packet)){
//setLed(1,1,1);
if(Packet->use.head.RPC != _PNG){
println_I("Got:");printPacket(Packet,INFO_PRINT);
}
if ( (CheckAddress(MyMAC.v,Packet->use.head.MAC.v) == true) || ((CheckAddress((uint8_t *)Broadcast.v,(uint8_t *)Packet->use.head.MAC.v) == true) )) {
float start=getMs();
Process_Self_Packet(Packet);
if(getMs()-start>5){
println_E("Process too long: ");p_fl_E(getMs()-start);
}
for (i=0;i<6;i++){
Packet->use.head.MAC.v[i]=MyMAC.v[i];
}
SetCRC(Packet);
start=getMs();
PutBowlerPacket(Packet);
if(getMs()-start>5){
println_E("Return too long: ");p_fl_E(getMs()-start);
}
if(Packet->use.head.RPC != _PNG){
println_I("Response:");printPacket(Packet,INFO_PRINT);
}
}else{
//println_I("Packet not addressed to me: ");printByteArray(Packet->use.head.MAC.v,6); print_I(" is not mine: ");printByteArray(MyMAC.v,6);
}
//setLed(0,0,1);
setPrintLevel(l);
return true;
}//Have a packet
setPrintLevel(l);
return false;
}
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();
}
}
/*****************************************************************************
Function:
uint32_t TickConvertToMilliseconds(uint32_t dwTickValue)
Summary:
Converts a Tick value or difference to milliseconds.
Description:
This function converts a Tick value or difference to milliseconds. For
example, TickConvertToMilliseconds(32768) returns 1000 when a 32.768kHz
clock with no prescaler drives the Tick module interrupt.
Precondition:
None
Parameters:
dwTickValue - Value to convert to milliseconds
Returns:
Input value expressed in milliseconds.
Remarks:
This function performs division on DWORDs, which is slow. Avoid using
it unless you absolutely must (such as displaying data to a user). For
timeout comparisons, compare the current value to a multiple or fraction
of TICK_SECOND, which will be calculated only once at compile time.
***************************************************************************/
float MyTickConvertToMilliseconds(float dwTickValue)
{
float ret;
do{
ret = (
(
( dwTickValue)+
(TICKS_PER_SECOND/2000ul)
)/
( (float) (TICKS_PER_SECOND/1000ul) )
);
if(isnan(ret)){
println_E("Timer NaN, recalculating..");
}
}while(isnan(ret));
return ret;
}
uint32_t calcTimer(uint32_t value){
if(value<=1)
value=1;
value = value * 8;
if(value>0x0000ffff){
println_E("Maxed timer to: ");prHEX32(value,ERROR_PRINT);
value = 0x0000ffff;
}
uint32_t target = value +currentTimer;
if(target>0x0000ffff){
target -=(0x0000ffff);
}
if(target < 5 ){
//println_E("Edge: ");prHEX32(target,ERROR_PRINT);
return 5;
}
return target & 0x0000ffff;
}
boolean setUpNextServo(){
int diff = positionTemp[sort[sortedIndex]] - lastValue;
lastValue = positionTemp[sort[sortedIndex]];
if(diff<0){
setPrintLevelErrorPrint();
println_E("Servo.c: Something is wrong!! Current minus last value is less then 0");
while(1);
}
if(diff>MIN_SERVO){
setTimerServoTicks(diff);
return true;
}
//Fall through for pin shut off
servoStateMachineCurrentState = TIME;
return false;
}
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 SetName(char * name) {
//WORD_VAL raw;
uint8_t i = 0;
println_E(name);
LoadCorePacket(&downstreamPacketTemp);
downstreamPacketTemp.use.head.Method = BOWLER_POST;
downstreamPacketTemp.use.head.RPC = GetRPCValue(eepd);
downstreamPacketTemp.use.data[0] = NAMESTART;
downstreamPacketTemp.use.data[1] = LOCKSTART;
while (name[i] != '\0') {
downstreamPacketTemp.use.data[2 + i] = name[i];
i++;
buttonCheck(9);
if (i == NAMESIZE)
break;
}
downstreamPacketTemp.use.data[2 + i] = '\0';
downstreamPacketTemp.use.head.DataLegnth = 6 + i + 1;
printPacket(&downstreamPacketTemp,ERROR_PRINT);
SendPacketToCoProc(&downstreamPacketTemp);
}
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 UserInit(void){
//setPrintStream(&USBPutArray);
setPrintLevelInfoPrint();
println_I("\n\nStarting PIC initialization");
//DelayMs(1000);
hardwareInit();
println_I("Hardware Init done");
ReleaseAVRReset();
CheckRev();
LoadEEstore();
LoadDefaultValues();
CartesianControllerInit();
InitPID();
UpdateAVRLED();
lockServos();
setPrintLevelInfoPrint();
BOOL brown = getEEBrownOutDetect();
setCoProcBrownOutMode(brown);
setBrownOutDetect(brown);
println_I("###Starting PIC In Debug Mode###\n");// All printfDEBUG functions do not need to be removed from code if debug is disabled
DelayMs(1000);
//setPrintLevelErrorPrint();
println_E("Error level printing");
println_W("Warning level printing");
println_I("Info level printing");
}
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();
}
boolean _getBowlerPacket(BowlerPacket * Packet,BYTE_FIFO_STORAGE * fifo, boolean debug){
boolean PacketCheck=false;
//uint16_t PacketLegnth=0;
Packet->stream[0]=0;
if (getNumBytes(fifo) == 0 ) {
return false; //Not enough bytes to even be a header, try back later
}
allign(Packet,fifo);
if (getNumBytes(fifo) < ((_BowlerHeaderSize)+4)) {
if(debug){
//b_println("Current num bytes: ",ERROR_PRINT);p_int(getNumBytes(fifo),ERROR_PRINT);
}
return false; //Not enough bytes to even be a header, try back later
}
FifoReadByteStream(Packet->stream,_BowlerHeaderSize,fifo);
PacketCheck=false;
while(PacketCheck==false) {
if( (Packet->use.head.ProtocolRevision != BOWLER_VERSION)
|| (CheckCRC(Packet)==false)
){
if(Packet->use.head.ProtocolRevision != BOWLER_VERSION){
b_println("first",ERROR_PRINT);
}else if(CheckCRC(Packet)==false) {
b_println("crc",ERROR_PRINT);
}
//prHEX8(Packet->use.head.ProtocolRevision,ERROR_PRINT);print_nnl(" Fifo Size=",ERROR_PRINT);p_int(calcByteCount(fifo),ERROR_PRINT);
uint8_t b;
if(getNumBytes(fifo)==0)
return false;
//StartCritical();
getStream(& b,1,fifo);//junk out one
//EndCritical();
FifoReadByteStream(Packet->stream,_BowlerHeaderSize,fifo);
}else{
if(debug){
//b_println("Got header");
}
PacketCheck=true;
}
if (getNumBytes(fifo) < minSize) {
b_println("allign packet",ERROR_PRINT);
allign(Packet,fifo);
return false; //Not enough bytes to even be a header, try back later
}
}
//PacketLegnth = Packet->use.head.DataLegnth;
uint16_t totalLen = GetPacketLegnth(Packet);
// See if all the data has arived for this packet
int32_t num = getNumBytes(fifo);
if (num >=(totalLen) ){
if(debug){
//b_println("**Found packet, ");p_int(totalLen);//print_nnl(" Bytes, pulling out of buffer");
}
//StartCritical();
getStream(Packet->stream,totalLen,fifo);
//EndCritical();
if(CheckDataCRC(Packet)){
return true;
}else{
println_E("Data CRC Failed ");printBowlerPacketDEBUG(Packet,ERROR_PRINT);
}
}
if(debug){
//b_println("Header ready, but data is not. Need: ",INFO_PRINT);p_int(totalLen,INFO_PRINT);print_nnl(" have: ",INFO_PRINT);p_int(num ,INFO_PRINT);
}
return false;
}
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;
}
