boolean processPIDGet(BowlerPacket * Packet){
int i;
switch (Packet->use.head.RPC){
case APID:
Packet->use.head.DataLegnth=5;
Packet->use.data[0]=getNumberOfPidChannels();
for(i=0;i<getNumberOfPidChannels();i++){
set32bit(Packet,GetPIDPosition(i),1+(i*4));
Packet->use.head.DataLegnth+=4;
}
Packet->use.head.Method=BOWLER_POST;
break;
case _PID:
set32bit(Packet,GetPIDPosition(Packet->use.data[0]),1);
Packet->use.head.DataLegnth=4+1+4;
Packet->use.head.Method=BOWLER_POST;
break;
case CPID:
GetConfigPID(Packet);
break;
case CPDV:
GetConfigPDVelocity(Packet);
break;
case GPDC:
set32bit(Packet,getNumberOfPidChannels(),0);
Packet->use.head.DataLegnth=4+4;
Packet->use.head.Method=BOWLER_POST;
break;
default:
return false;
}
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;
}
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;
}
boolean GetChanelValueFromPacket(BowlerPacket * Packet) {
uint8_t pin = Packet->use.data[0];
uint8_t mode = GetChannelMode(pin);
uint8_t numValues = 1;
int32_t data;
if (getChanelValueHWPtr != NULL) {
getChanelValueHWPtr(pin,
&numValues,
&data);
} else {
return false;
}
if(isOutputMode(GetChannelMode(pin))==false ){
setDataTableCurrentValue(pin,data);
}else{
if(mode== IS_UART_RX){
setDataTableCurrentValue(pin,data);
}else{
data = getBcsIoDataTable(pin)->PIN.currentValue;
}
}
set32bit(Packet, data, 1);
numValues = 4;
Packet->use.head.DataLegnth = 4 + 1 + numValues;
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;
}
void _SetChannelValueCoProc(uint8_t PIN, uint8_t state) {
uint8_t retry = 0;
do {
if (retry > 0) {
//println_E("#*#*SetChannelValueCoProc did not return RDY pin: ");p_int_E(PIN);print_E(" mode: ");printMode(GetChannelMode(PIN),ERROR_PRINT);
//printPacket(&downstreamPacketTemp,ERROR_PRINT);
return;
}
LoadCorePacket(&downstreamPacketTemp);
downstreamPacketTemp.use.head.Method = BOWLER_POST;
downstreamPacketTemp.use.head.RPC = GetRPCValue("schv");
downstreamPacketTemp.use.data[0] = PIN;
set32bit(&downstreamPacketTemp, state, 1);
set32bit(&downstreamPacketTemp, 0, 5); // zero ms
downstreamPacketTemp.use.head.DataLegnth = 4 + 1 + 4 + 4;
SendPacketToCoProc(&downstreamPacketTemp);
setDataTableCurrentValue(PIN, state);
retry++;
buttonCheck(13);
} while (downstreamPacketTemp.use.head.RPC != _RDY);
}
uint8_t SetCoProConfiguration(uint8_t pin, int32_t mode) {
LoadCorePacket(&downstreamPacketTemp);
downstreamPacketTemp.use.head.Method = BOWLER_CRIT;
downstreamPacketTemp.use.head.RPC = GetRPCValue("cchn");
downstreamPacketTemp.use.data[0] = pin;
downstreamPacketTemp.use.data[1] = true;
downstreamPacketTemp.use.data[2] = 1;
set32bit(&downstreamPacketTemp, mode, 3);
downstreamPacketTemp.use.head.DataLegnth = 4 + 3 + 4;
SendPacketToCoProc(&downstreamPacketTemp);
return false;
}
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;
}
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;
}
BOOL GetChanelValueFromPacket(BowlerPacket * Packet){
BYTE pin = Packet->use.data[0];
BYTE mode = GetChannelMode(pin);
BYTE numValues;
if(isStremChannelMode(mode)){
if(getChanelValueHWPtr!=NULL){
// Load the data directly into the packet as the buffer
//Data pointer is offset by one to start after the pin index
getChanelValueHWPtr(pin,
&numValues,
(INT32 *)(Packet->use.data+1));
}else{
return FALSE;
}
}else{
INT32 data;
if(getChanelValueHWPtr!=NULL){
getChanelValueHWPtr(pin,
&numValues,
&data);
}else{
return FALSE;
}
if(isSingleByteMode(mode)){
set8bit(Packet,data, 1);
numValues=1;
}else if(isTwoByteMode(mode)){
set16bit(Packet,data, 1);
numValues=2;
}else{
set32bit(Packet,data, 1);
numValues=4;
}
}
Packet->use.head.DataLegnth = 4+1+numValues;
FixPacket(Packet);
return TRUE;
}
uint8_t GetCoProConfigurations() {
LoadCorePacket(&downstreamPacketTemp);
downstreamPacketTemp.use.head.Method = BOWLER_CRIT;
downstreamPacketTemp.use.head.RPC = GetRPCValue("cchn");
downstreamPacketTemp.use.data[0] = 0xff;
downstreamPacketTemp.use.data[1] = false;
downstreamPacketTemp.use.data[2] = 1;
set32bit(&downstreamPacketTemp, 0, 3);
downstreamPacketTemp.use.head.DataLegnth = 4 + 3 + 4;
SendPacketToCoProc(&downstreamPacketTemp);
int i;
for (i = 0; i < GetNumberOfIOChannels(); i++) {
down[i].currentConfiguration = get32bit(&downstreamPacketTemp,
1 + (i * 4));
//println_E(__FILE__);println_E("GetCoProConfigurations");
//setDataTableCurrentValue(i,down[i].currentConfiguration);
}
return false;
}
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;
}
