boolean GetAllChanelValueFromPacket(BowlerPacket * Packet) {
int32_t * data = (int32_t *) (&Packet->use.data[1]);
if (getAllChanelValueHWPtr != NULL) {
int i;
int32_t tmp;
getAllChanelValueHWPtr(data);
for (i = 0; i < GetNumberOfIOChannels(); i++) {
tmp = data[i];
if(isOutputMode(GetChannelMode(i))==false){
setDataTableCurrentValue(i,tmp);
}else{
if(GetChannelMode(i) == IS_SERVO){
tmp = GetServoPos(i);
}else if(GetChannelMode(i)== IS_UART_RX){
setDataTableCurrentValue(i,tmp);
}else{
tmp = getBcsIoDataTable(i)->PIN.currentValue;
}
}
set32bit(Packet, tmp, (i*4)+1);
}
Packet->use.data[0]=(GetNumberOfIOChannels());
Packet->use.head.RPC=GetRPCValue("gacv");
Packet->use.head.DataLegnth = 4+1+(GetNumberOfIOChannels()*4);
} else
return false;
FixPacket(Packet);
return true;
}
BOOL GetAllChannelModeFromPacket(BowlerPacket * Packet){
int i;
Packet->use.head.Method=BOWLER_POST;
for (i=0;i<GetNumberOfIOChannels();i++){
Packet->use.data[i]=GetChannelMode(i);
}
Packet->use.head.DataLegnth=4+GetNumberOfIOChannels();
FixPacket(Packet);
return TRUE;
}
boolean GetAllChannelModeFromPacket(BowlerPacket * Packet) {
int i;
Packet->use.head.Method = BOWLER_POST;
Packet->use.head.RPC=GetRPCValue("gacm");
Packet->use.data[0] = GetNumberOfIOChannels();
for (i = 0; i < GetNumberOfIOChannels(); i++) {
Packet->use.data[1 + i] = GetChannelMode(i);
}
Packet->use.head.DataLegnth = 4 + GetNumberOfIOChannels()+1;
FixPacket(Packet);
//printBowlerPacketDEBUG(Packet,WARN_PRINT);
return true;
}
/**
* Set Channel Values
* This function takes a
* @param data an array of data values
* @param ms the time for the transition to take
*
*/
BOOL SetAllChanelValueHW(INT32 * data, float ms){
int i;
for(i=0;i<GetNumberOfIOChannels();i++){
if(!isStremChannelMode(GetChannelMode(i)))
SetChanelValueHW(i,1,& data[i], ms);
}
return TRUE;
}
BOOL GetAllChanelValueFromPacket(BowlerPacket * Packet){
INT32 * data = (INT32 *)(Packet->use.data);
if(getAllChanelValueHWPtr!=NULL){
int i;
INT32 tmp;
getAllChanelValueHWPtr((INT32 * )Packet->use.data);
for(i=0;i<GetNumberOfIOChannels();i++){
tmp = data[i];
getBcsIoDataTable()[i].PIN.currentValue = tmp;
set32bit(Packet,tmp,i*4);
}
Packet->use.head.DataLegnth = 4+GetNumberOfIOChannels()*4;
}else
return FALSE;
FixPacket(Packet);
return TRUE;
}
void UserInit(void){
StartCritical();
//println_W(startmessage);// All printfDEBUG functions do not need to be removed from code if debug is disabled
//#if defined(DEBUG)
// ConfigureUART(115200);
// if(GetChannelMode(16)!=IS_UART_TX)
// setMode(16,IS_UART_TX);
//#endif
setPrintLevelInfoPrint();
println_I(/*PSTR*/("\e[1;1H\e[2J ***Starting User initialization***"));
InitFlagPins();
InitBankLEDs();
SetPowerState0(0,0);
SetPowerState1(0,0);
//_delay_ms(1);
#if defined(WPIRBE)
SPISlaveInit();
#endif
//_delay_ms(100);
println_I(/*PSTR*/("Starting Pin Functions"));
InitPinFunction();
println_I(/*PSTR*/("Starting Pin Modes"));
InitPinModes();
int i=0;
//println_I(/*PSTR*/("Starting hardware modes"));
for(i=0;i<GetNumberOfIOChannels();i++){
initPinState(i);
configAdvancedAsyncNotEqual(i,10);
setAsyncLocal(i,true) ;
}
//println_I(/*PSTR*/("DONE pin initialization"));
//println_I(/*PSTR*/("Adding IO Initialization"));
addNamespaceToList((NAMESPACE_LIST *)get_bcsIoNamespace());
//println_I(/*PSTR*/("Adding IO.SETMODE Initialization"));
addNamespaceToList((NAMESPACE_LIST *)get_bcsIoSetmodeNamespace());
//println_I(/*PSTR*/("Adding Internal Initialization"));
addNamespaceToList((NAMESPACE_LIST *)get_internalNamespace());
setNoAsyncMode(true);
setIgnoreAddressing(true);
//SetPinTris(0,OUTPUT);
//SetDIO(0,OFF);
#if defined(USE_AS_LIBRARY)
InitializeUserCode();
#endif
EndCritical();
println_I(/*PSTR*/("Starting Core"));
// setMode(22,IS_DO);
// setMode(23,IS_DI);
// println_I(/*PSTR*/("Pin done"));
}
void printValues(){
int i;
println_I("Values");
for(i=0;i<GetNumberOfIOChannels();i++){
println_I("\t# ");p_int_I(i);
print_I("\tCurrent ");p_int_I(getBcsIoDataTable()[i].PIN.currentValue);
//print_I("\tPrevious ");p_int_I(getBcsIoDataTable()[i].PIN.previousValue);
}
}
void printConfigurations(){
int i;
println_I("Configurations");
for(i=0;i<GetNumberOfIOChannels();i++){
println_I("\t# ");p_int_I(i);
//print_I("\tCurrent ");p_int_I(getBcsIoDataTable()[i].PIN.currentConfiguration);
//print_I("\tPrevious ");p_int_I(getBcsIoDataTable()[i].PIN.previousConfiguration);
}
}
/**
* Set Channel Values
* This function takes a pin index, a number of values to be delt with, and an array of data values
* Data is stored into numValues and data
*/
BOOL GetAllChanelValueHW(INT32 * data){
int i;
BYTE numValues;
for(i=0;i<GetNumberOfIOChannels();i++){
if(!isStremChannelMode(GetChannelMode(i)))
GetChanelValueHW(i,&numValues,& data[i]);
}
return TRUE;
}
void printModes(){
int i;
println_I("Modes");
for(i=0;i<GetNumberOfIOChannels();i++){
println_I("\t# ");p_int_I(i);
print_I("\tCurrent ");printMode(getBcsIoDataTable()[i].PIN.currentChannelMode,INFO_PRINT);
//print_I("\tPrevious ");printMode(getBcsIoDataTable()[i].PIN.previousChannelMode,INFO_PRINT);
}
}
/*
* Set All channel modes
* Iterates down the array of channel modes setting all of them
* Returns true if all modes set OK
*/
boolean SetAllChannelModes(uint8_t * modeArray){
int i=0;
for(i=0;i<GetNumberOfIOChannels();i++){
if(!SetChannelMode(i,modeArray[i])){
return false;
}
}
return true;
}
void SyncConfigurations() {
int i;
for (i = 0; i < GetNumberOfIOChannels(); i++) {
if (down[i].changeConfiguration == true) {
down[i].changeConfiguration = false;
SetCoProConfiguration(i, down[i].currentConfiguration);
}
}
}
BOOL GetIOChannelCountFromPacket(BowlerPacket * Packet){
Packet->use.head.Method=BOWLER_POST;
Packet->use.data[0]=0;
Packet->use.data[1]=0;
Packet->use.data[2]=0;
Packet->use.data[3]=GetNumberOfIOChannels();;
Packet->use.head.DataLegnth=4+4;
FixPacket(Packet);
return TRUE;
}
void InitPinStates(void){
SyncModes();
println_I("Modes synced, initializing channels");
initAdvancedAsync();
int i;
for (i=0;i<GetNumberOfIOChannels();i++){
//DelayMs(10);
SetChannelMode(i,GetChannelMode(i));
}
}
/*
* Set All channel modes
* Iterates down the array of channel modes setting all of them
* Returns true if all modes set OK
*/
boolean AbstractSetAllChannelMode(BowlerPacket * Packet){
// First byte is the number of channels
//printBowlerPacketDEBUG(Packet,WARN_PRINT);
int i;
for(i=0;i<GetNumberOfIOChannels();i++){
SetChannelMode(i,Packet->use.data[i+1]);
}
GetAllChannelModeFromPacket(Packet);
return true;
}
boolean GetIOChannelCountFromPacket(BowlerPacket * Packet) {
Packet->use.head.Method = BOWLER_POST;
Packet->use.data[0] = 0;
Packet->use.data[1] = 0;
Packet->use.data[2] = 0;
Packet->use.data[3] = GetNumberOfIOChannels();
Packet->use.head.RPC= GetRPCValue("gchc");
Packet->use.head.DataLegnth = 4 + 4;
FixPacket(Packet);
return true;
}
boolean ConfigureChannelFromPacket(BowlerPacket * Packet) {
uint8_t pin = Packet->use.data[0];
boolean setValues = Packet->use.data[1];
uint8_t mode = GetChannelMode(pin);
int32_t tmp;
if(mode != 0xff && setValues){
if (configChannelHWPtr != NULL) {
// println_E("Pushing configs from packet ");
// p_int_E(pin);
tmp = get32bit(Packet, 3);
// print_E(" value = ");
// p_int_E(tmp);
//println_E(__FILE__);println_E("ConfigureChannelFromPacket");
setDataTableCurrentValue(pin,tmp);
configChannelHWPtr(pin, 1, &tmp);
} else {
return false;
}
}
//println_E("Loading configs into packet");
Packet->use.head.RPC= GetRPCValue("cchn");
Packet->use.head.Method = BOWLER_CRIT;
Packet->use.head.DataLegnth = 4+1+(GetNumberOfIOChannels()*4);
int32_t * data = (int32_t *) (Packet->use.data + 1);
configChannelHWPtr(0xff, GetNumberOfIOChannels(), data);
Packet->use.data[0] =GetNumberOfIOChannels();
int i;
for (i = 0; i < GetNumberOfIOChannels(); i++) {
tmp = data[i];
set32bit(Packet, tmp, (i*4)+1);
}
FixPacket(Packet);
return true;
}
uint8_t SetAllCoProcValues() {
int i = 0;
boolean send = true;
int32_t tmp;
if (send) {
LoadCorePacket(&downstreamPacketTemp);
downstreamPacketTemp.use.head.Method = BOWLER_POST;
downstreamPacketTemp.use.head.RPC = GetRPCValue("sacv");
set32bit(&downstreamPacketTemp, 0, 0); // setting the translation time
downstreamPacketTemp.use.data[4] = GetNumberOfIOChannels();
for (i = 0; i < GetNumberOfIOChannels(); i++) {
tmp = getBcsIoDataTable(i)->PIN.currentValue;
down[i].changeValue = false;
set32bit(&downstreamPacketTemp, tmp, (i * 4) + 5);
}
downstreamPacketTemp.use.head.DataLegnth = 4 + 4 + 1
+ (4 * GetNumberOfIOChannels());
SendPacketToCoProc(&downstreamPacketTemp);
}
for (i = 0; i < GetNumberOfIOChannels(); i++) {
int index = (i * 4) + 1;
if (isOutputMode(GetChannelMode(i)) == false) {
tmp= get32bit(&downstreamPacketTemp, index);
boolean back = (tmp != getBcsIoDataTable(i)->PIN.currentValue);
setDataTableCurrentValue(i,tmp);
if (back) {
getBcsIoDataTable(i)->PIN.asyncDataenabled = true;
}
}
if (GetChannelMode(i) == IS_SERVO) {
SetServoPos(i, get32bit(&downstreamPacketTemp, index) & 0x000000ff);
}
if (GetChannelMode(i) == IS_UART_RX) {
}
}
return true;
}
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 ;
}
void printAsync(){
int i;
println_I("Async Data ");p_fl_I(getMs());
for(i=0;i<GetNumberOfIOChannels();i++){
println_I("\t# ");p_int_I(i);
print_I("\tCurrent ");p_int_I(getBcsIoDataTable()[i].PIN.asyncDataCurrentVal);
print_I("\tPrevious ");p_int_I(getBcsIoDataTable()[i].PIN.asyncDataPreviousVal);
print_I("\tMode ");printAsyncType(getBcsIoDataTable()[i].PIN.asyncDataType);
print_I("\tIteration ");p_fl_I(getBcsIoDataTable()[i].PIN.asyncDataTime.setPoint);
print_I("\tLast ");p_fl_I(getBcsIoDataTable()[i].PIN.asyncDataTime.MsTime);
}
}
boolean SetAllChannelValueFromPacket(BowlerPacket * Packet) {
int32_t * data = (int32_t *) (&Packet->use.data[5] );
int32_t tmp;
if (setAllChanelValueHWPtr != NULL) {
uint32_t time;
uint8_t i;
time=get32bit(Packet, 0);
for (i = 0; i < GetNumberOfIOChannels(); i++) {
if(isOutputMode(GetChannelMode(i))==true){
tmp = get32bit(Packet, (i*4) +5);
// if(GetChannelMode(i) == IS_SERVO)
// data[i] = (tmp & 0x000000ff) | (time<<16);
// else
data[i] = tmp;
}else{
data[i] = getBcsIoDataTable(i)->PIN.currentValue;
}
}
setAllChanelValueHWPtr(data, time);
for (i = 0; i < GetNumberOfIOChannels(); i++) {
if(isOutputMode(GetChannelMode(i))==true){
//println_E(__FILE__);println_E("SetAllChannelValueFromPacket");
if(GetChannelMode(i)!=IS_SERVO)
setDataTableCurrentValue(i,data[i]);
}
}
//READY(Packet, 3, 3);
GetAllChanelValueFromPacket(Packet);
} else {
return false;
}
return true;
}
void ConfigPPM(BowlerPacket * Packet){
int i;
for(i=0;i<NUM_PPM_CHAN;i++){
ppmLink[i]=Packet->use.data[i+1];
if(ppmLink[i]>= GetNumberOfIOChannels()){
ppmLink[i]=INVALID_PPM_LINK;
}else{
unlockServos();
}
}
writeLinks=true;
READY(Packet,66,0);
}
void ConfigPPMFromArray(uint8_t * data){
int i;
for(i=0;i<NUM_PPM_CHAN;i++){
ppmLink[i]=data[i];
if(ppmLink[i]>= GetNumberOfIOChannels()){
ppmLink[i]=INVALID_PPM_LINK;
}else{
unlockServos();
}
}
writeLinks=true;
}
void startPPM(uint8_t chan){
int i;
if(chan == 23){
configPin23Int();
state = START;
for(i=0;i<NUM_PPM_CHAN;i++){
if(ppmLink[i]>= GetNumberOfIOChannels())
ppmLink[i]=INVALID_PPM_LINK;
}
//println_I("Starting PPM reader");
}
}
void InitPins(void){
//println_I(/*PSTR*/("Starting Pin Functions"));
InitPinFunction();
//println_I(/*PSTR*/("Starting Pin Modes"));
InitPinModes();
int i=0;
//println_I(/*PSTR*/("Starting hardware modes"));
for(i=0;i<GetNumberOfIOChannels();i++){
initPinState(i);
configAdvancedAsyncNotEqual(i,10);
setAsyncLocal(i,TRUE);
}
//println_I(/*PSTR*/("DONE pin initialization"));
}
void initAdvancedAsync(){
if(isInit == true) {
//println_W("All ready initialized advanced async");
return;
}
isInit=true;
int i;
for (i=0;i<GetNumberOfIOChannels();i++){
startAdvancedAsyncDefault(i);
}
//println_W("Initializing advanced async");
}
uint8_t SetAllCoProcMode() {
int i = 0;
boolean send = false;
for (i = 0; i < GetNumberOfIOChannels(); i++) {
if (down[i].changeMode == true) {
send = true;
}
}
if (send) {
LoadCorePacket(&downstreamPacketTemp);
downstreamPacketTemp.use.head.Method = BOWLER_POST;
downstreamPacketTemp.use.head.RPC = GetRPCValue("sacm");
downstreamPacketTemp.use.data[0] = GetNumberOfIOChannels();
for (i = 0; i < GetNumberOfIOChannels(); i++) {
downstreamPacketTemp.use.data[i + 1] =GetChannelMode(i) ;
down[i].changeMode = false;
}
downstreamPacketTemp.use.head.DataLegnth = 4 + 1+ GetNumberOfIOChannels();
//println_I("Sending mode sync packet");printPacket(&downstreamPacketTemp,WARN_PRINT);
SendPacketToCoProc(&downstreamPacketTemp);
// for (i = 0; i < NUM_PINS; i++) {
// if (downstreamPacketTemp.use.data[i + 1] == NO_CHANGE) {
// //getBcsIoDataTable(i)->PIN.currentChannelMode = IS_DI;
// SetChannelModeDataTable(i,IS_DI);
// down[i].changeMode = true; // force a sync of the no valid mode
// //println_E("FAULT: the mode was set to NO_CHANGE");
// } else {
// //getBcsIoDataTable(i)->PIN.currentChannelMode =downstreamPacketTemp.use.data[i + 1];
// SetChannelModeDataTable(i,downstreamPacketTemp.use.data[i + 1]);
// down[i].changeMode = false;
// }
// }
}
return true;
}
/**
* Sets the datable value and returns true if the value is new, false if it is the same as it was
*/
boolean _setDataTableCurrentValue(uint8_t pin, int32_t value){
if(pin>=GetNumberOfIOChannels()){
//println_E("Pin out of index! : "); p_int_E(pin);
}
if(value !=getBcsIoDataTable(pin)->PIN.currentValue ){
//Print_Level l = INFO_PRINT;
// Print_Level l = isOutputMode(GetChannelMode(pin))?ERROR_PRINT:INFO_PRINT;
// println(" Value was ",l);p_int(getBcsIoDataTable(pin)->PIN.currentValue,l);
// print_nnl(" set to ",l);p_int(value,l);
// print_nnl(" on pin ",l);p_int(pin,l);
// print_nnl(" mode ",l);printMode(GetChannelMode(pin),l);
// THis is the only place this variable should be set
getBcsIoDataTable(pin)->PIN.currentValue =value;
// print_nnl(" lastPushed ",l);p_int( getBcsIoDataTable(pin)->PIN.asyncDataPreviousVal,l);
return true;
}
return false;
}
boolean checkDigital(){
int i;
for(i=0;i<GetNumberOfIOChannels();i++){
uint8_t mode = GetChannelMode(i);
boolean run = (((mode == IS_DI) ) || ((mode == IS_COUNTER_INPUT_HOME)||(mode == IS_COUNTER_OUTPUT_HOME) || (mode == IS_SERVO)));
if (run){
if(mode == IS_SERVO){
//setDataTableCurrentValue(i,GetServoPos(i));
}else{
if(GetValFromAsync(i) != GetDIO(i)){
setDataTableCurrentValue(i,GetDIO(i));
//printAsync();
}
}
}
if(mode == IS_UART_RX){
setDataTableCurrentValue(i,Get_UART_Byte_CountPassThrough());
}
}
return true;
}
void LoadPIDvals(AbsPID * pid, DYIO_PID * dy){
LoadEEstore();
BYTE i = pid->channel;
if(pidEEPRomVal[i].outputChannel==pidEEPRomVal[i].inputChannel)
return;
if(pidEEPRomVal[i].outputChannel>=GetNumberOfIOChannels() ||pidEEPRomVal[i].inputChannel>=GetNumberOfIOChannels() )
return;
if(pidEEPRomVal[i].outputMode==pidEEPRomVal[i].inputMode)
return;
println_I("Using values for chan: ");p_int_I(i);
pid->Enabled=pidEEPRomVal[i].Enabled;
pid->Polarity=pidEEPRomVal[i].Polarity;
//pidChans->Async=pidEEPRomVal[i].Async;
dy->inputMode=pidEEPRomVal[i].inputMode;
dy->outputMode=pidEEPRomVal[i].outputMode;
dy->outputChannel=pidEEPRomVal[i].outputChannel;
dy->inputChannel=pidEEPRomVal[i].inputChannel;
pid->K.P=pidEEPRomVal[i].K.P;
pid->K.I=pidEEPRomVal[i].K.I;
pid->K.D=pidEEPRomVal[i].K.D;
}