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;
}
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;
}
/**
* Initialize the SPI port to work as an SPI channel
*/
void SPISlaveInit(){
//println_I("\t\tStarting the SPI slave mode");
DDRBbits._P4=INPUT;//Set the SS to an input
SPIDDRbits._MOSI=INPUT;// Master out
SPIDDRbits._SCK=INPUT; // Clock
SPIDDRbits._MISO=OUTPUT; // Master in
PRRbits._PRSPI=0; //Disable powersave features.
SPCRbits._SPIE=1; //Enable Interupt
SPCRbits._SPE=1; //Enable SPI
SPCRbits._DORD=0; //Most signifigant bit first
SPCRbits._MSTR=0; //Set as slave
SPCRbits._CPOL=0; //Idle clock at 0, rising edge on leading edge
SPCRbits._CPHA=0; //Clock phase leading edge sample
SPDR=0xff;
InitByteFifo(&storeTX,privateTX,sizeof(privateTX));
InitByteFifo(&storeRX,privateRX,sizeof(privateRX));
SetPowerState(TRUE,FALSE,false) ;
uint8_t i;
for(i=8;i<16;i++){
if(GetChannelMode(i)==IS_DI||GetChannelMode(i)==IS_DO)
setMode(i,IS_SERVO);
}
//setMode(8,IS_DO);
}
int GetDigitalValFromAsync(uint8_t pin){
initAdvancedAsync();
if(GetChannelMode(pin)==IS_DI || GetChannelMode(pin)==IS_COUNTER_INPUT_HOME || GetChannelMode(pin)==IS_COUNTER_OUTPUT_HOME ){
return getDataTableCurrentValue(pin);
}
if(GetChannelMode(pin)== IS_SERVO){
return GetServoPos(pin);
}
return 1;
}
void SetServoPos(uint8_t pin,uint8_t val,float time){
if(time<30)
time=0;
if(val == velocity[pin].set){
return;
}
//
// println_W("Servo ");p_int_W(pin);
// print_W(" time= ");p_fl_W(time);
// print_W(" to val= ");p_int_W(val);
// print_W(" val was= ");p_fl_W(velocity[pin].set);
SetServoPosDataTable( pin, val, time);
getInterpolatedPin( pin);
if(pin<12){
blockServo[0]=false;
}else{
blockServo[1]=false;
}
if(GetChannelMode(pin)!=IS_SERVO)
return;
print = pin;
}
void SendPacketToSPI(BowlerPacket * Packet){
if(!isSPI(GetChannelMode(Packet->use.data[0]))){
println_I("channel is not SPI");
return;
}
SendPacketToSPIFromArray(Packet->use.head.DataLegnth-6,Packet->use.data+1);
}
//char * unknown = "UNKNOWN";
void printMode(uint8_t pin,uint8_t newMode, Print_Level l){
uint8_t mode =GetChannelMode(pin);
printModeLocal( mode, l);
print_nnl(" ,",l);
printModeLocal( newMode, l);
}
BOOL ConfigureChannelFromPacket(BowlerPacket * Packet){
BYTE pin = Packet->use.data[0];
BYTE mode = GetChannelMode(pin);
INT32 * data = (INT32 *)(Packet->use.data+1);
INT32 tmp;
int i;
if(configChannelHWPtr!=NULL){
if(Packet->use.head.DataLegnth>5 && mode != IS_SERVO){
int numVals = (Packet->use.head.DataLegnth-(4+1))/4;
for(i=0;i<numVals;i++){
tmp = get32bit(Packet, (i*4)+1);
getBcsIoDataTable()[i].PIN.currentValue = tmp;
data[i]=tmp;
}
configChannelHWPtr(pin,numVals,(INT32 * )(Packet->use.data+1));
}else{
// Single Byte Servo, legacy HACK
INT32 value = Packet->use.data[1];
configChannelHWPtr(pin,1,&value);
}
}else{
return FALSE;
}
FixPacket(Packet);
return TRUE;
}
BOOL GetChannelModeFromPacket(BowlerPacket * Packet){
Packet->use.data[1]=GetChannelMode(Packet->use.data[0]);
Packet->use.head.DataLegnth=6;
Packet->use.head.Method=BOWLER_POST;
FixPacket(Packet);
return TRUE;
}
uint8_t GetPIDGroup(uint8_t channel){
uint8_t mode = GetChannelMode(channel);
uint8_t i;
switch(mode){
case IS_COUNTER_INPUT_INT:
case IS_COUNTER_INPUT_DIR:
case IS_COUNTER_INPUT_HOME:
case IS_ANALOG_IN:
case IS_DI:
for(i=0;i<NUM_PID_GROUPS;i++){
if(dyPid[i].inputChannel == channel){
if(pidGroups[i].config.Enabled || pidGroups[i].vel.enabled )
return i;
}
}
break;
case IS_SERVO:
case IS_DC_MOTOR_DIR:
case IS_DC_MOTOR_VEL:
case IS_PWM:
case IS_DO:
for(i=0;i<NUM_PID_GROUPS;i++){
if(dyPid[i].outputChannel == channel){
if(pidGroups[i].config.Enabled || pidGroups[i].vel.enabled)
return i;
}
}
break;
default:
break;
}
return NOT_USED_IN_PID;
}
int GetValFromAsync(int pin){
if(GetChannelMode(pin)== IS_SERVO){
return GetServoPos(pin);
}else{
return getDataTableCurrentValue(pin);
}
}
UINT16 GetChanVal(BYTE pin){
UINT16 val;
BYTE mode = GetChannelMode(pin);
switch (mode){
case HIGH_IMPEDANCE:
val=1;
break;
case IS_DI:
case IS_DO:
case IS_COUNTER_INPUT_HOME:
case IS_COUNTER_OUTPUT_HOME:
val = GetDIO(pin);
break;
case IS_SERVO:
val = GetServoPos(pin);
break;
case IS_PWM:
val = GetPWM(pin);
break;
case IS_DC_MOTOR_VEL:
case IS_DC_MOTOR_DIR:
val = GetDCMotor(pin);
break;
case IS_ANALOG_IN:
val=GetADC(pin);
break;
default:
val=1;
break;
}
return val;
}
/**
* 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;
}
boolean GetChannelModeFromPacket(BowlerPacket * Packet) {
Packet->use.data[1] = GetChannelMode(Packet->use.data[0]);
Packet->use.head.DataLegnth = 6;
Packet->use.head.Method = BOWLER_POST;
FixPacket(Packet);
//printBowlerPacketDEBUG(Packet,WARN_PRINT);
return true;
}
BOOL isAnalog(BowlerPacket * Packet){
if( (Packet->use.head.RPC==GCHV) &&
(GetChannelMode(Packet->use.data[0])==IS_ANALOG_IN)
){
return TRUE;
}
return FALSE;
}
boolean checkAnalog(){
int i=0;
for(i=8;i<16;i++){
if ((GetChannelMode(i) == IS_ANALOG_IN) ){
setDataTableCurrentValue(i, GetADC(i));
}
}
return true;
}
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;
}
/**
* 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 initPinState(uint8_t i){
uint8_t mode =GetChannelMode(i);
if ((mode == IS_ANALOG_IN) ){
setDataTableCurrentValue(i,GetADC(i));
}else if(((mode == IS_DI) ) || ((mode == IS_COUNTER_INPUT_HOME)||(mode == IS_COUNTER_OUTPUT_HOME))){
setDataTableCurrentValue(i,GetDIO(i)?1:0);
}else{
//setDataTableCurrentValue(i,0);
}
}
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));
}
}
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;
}
/**
* Legacy
*
*
*/
BOOL SaveValue(BYTE pin,BYTE val){
switch(GetChannelMode(pin)){
case IS_SERVO:
case IS_PWM:
EEWriteValue(pin,val);
return TRUE;
default:
return FALSE;
}
}
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 RunPPMCheck(void){
if(GetChannelMode(23) != IS_PPM_IN){
//setMode(23,IS_PPM_IN);
return;
}
if(startedLinks==false){
startedLinks=true;
readPPMLink(ppmLink);
}
if(writeLinks==true){
writeLinks=false;
writePPMLink(ppmLink);
}
int i;
boolean up = false;
for(i=0;i<NUM_PPM_CHAN;i++){
float fTime =MyTickConvertToMilliseconds(((float)ppmDataTmp[i])+(((float)TickGetUpper())*((float) 4294967295ul )));
if(fTime>.9 && fTime<2.2){
float fVal = (fTime-.99)*255;
if(fVal>254)
fVal=254;
if(fVal<0)
fVal=0;
uint8_t time =((BYTE) fVal);
if((time>(ppmData[i]+3)) || (time<(ppmData[i]-3))){
ppmData[i]=time;
//setHeartBeatState( FALSE, 0);
up=true;
}
}
}
if(up){
//push upstream
pushPPMPacket();
//print_I("\nUpdating ppm chan ");printStream(ppmData,NUM_PPM_CHAN);
//print_I(" PPM cross link ");printStream(ppmLink,NUM_PPM_CHAN);
for(i=0;i<NUM_PPM_CHAN;i++){
if(ppmLink[i] != INVALID_PPM_LINK){
if(ppmLastSent[i] != ppmData[i]){
ppmLastSent[i] = ppmData[i];
Print_Level l = getPrintLevel();
setPrintLevelInfoPrint();
//println_I("PPM setting output");
//SetChannelValueCoProc(ppmLink[i],ppmData[i]);
getBcsIoDataTable(ppmLink[i])->PIN.currentValue=ppmData[i];
//SetValFromAsync(ppmLink[i],ppmData[i]);
setPrintLevel(l);
}
}
}
}
}
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;
}
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;
}
void StopSPI(BYTE pin){
if (isSPI(GetChannelMode(pin))){
CloseSPI2();
_RG6=1;
_RG8=1;
SDI_TRIS=INPUT;
SDO_TRIS=INPUT;
SCK_TRIS=INPUT;
println_I("Clearing up SPI perpheral");
SetCoProcMode(0,IS_DI);
SetCoProcMode(1,IS_DI);
SetCoProcMode(2,IS_DI);
}
}
boolean IsAsync(uint8_t channel){
if (getBcsIoDataTable(channel)->PIN.asyncDataenabled){
return true;
}
uint8_t mode=GetChannelMode(channel);
switch(mode){
case IS_COUNTER_INPUT_HOME:
case IS_COUNTER_OUTPUT_HOME:
case IS_COUNTER_OUTPUT_INT:
case IS_COUNTER_INPUT_INT:
case IS_SERVO:
return true;
default:
return false;
}
}
BOOL SetChanelValueFromPacket(BowlerPacket * Packet){
BYTE pin = Packet->use.data[0];
BYTE mode = GetChannelMode(pin);
if(isStremChannelMode(mode)){
if(setChanelValueHWPtr!=NULL)
// Load the data directly into the packet as the buffer
//Data pointer is offset by one to start after the pin index
setChanelValueHWPtr(pin,
Packet->use.head.DataLegnth-(4+1),
(INT32 *)(Packet->use.data+1),
(float)0);
READY(Packet,1,3);
}else{
INT32 data = 0;
INT32 time = 0;
if(isSingleByteMode(mode)){
data = Packet->use.data[1];
if (Packet->use.head.DataLegnth>(4+2)){
time = get16bit(Packet,2);
}else{
time=0;
}
}else if(isTwoByteMode(mode)){
data=get16bit(Packet,1);
if (Packet->use.head.DataLegnth>(4+3)){
time= get32bit(Packet,3);
}else{
time=0;
}
}else{
data=get32bit(Packet,1);
if (Packet->use.head.DataLegnth>(4+5)){
time= get32bit(Packet,5);
}else{
time=0;
}
}
getBcsIoDataTable()[pin].PIN.currentValue = data;
if(setChanelValueHWPtr!=NULL)
setChanelValueHWPtr(pin,1,&data,(float)time);
READY(Packet,2,3);
}
return TRUE;
}
