void InitServo(uint8_t PIN){
//println_I("Starting servo");
ClearPinState(PIN);
SetPinTris(PIN,OUTPUT);
//DATA.PIN[PIN].State=IS_SERVO;
SetServoPos(PIN,EEReadValue(PIN),0);
if( servoEngineStarted != true){
servoEngineStarted =true;
startServoLoops();
}
}
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 InitPinModes(void){
BYTE i;
BYTE mode=0;
for (i=0;i<24;i++){
//ClearPinState(i);
SetPinTris(i,INPUT);
SetDIO(i,0);
SetPWM(i,EEReadValue(i));
SetServoPos(i,EEReadValue(i),0);
//getBcsIoDataTable()[i].PIN.previousChannelMode=NO_CHANGE;
mode=EEReadMode(i);
if((mode < 2)||(mode >=IO_MODE_MAX)){
EEWriteMode(i,IS_DI);
mode = EEReadMode(i);
}
getBcsIoDataTable()[i].PIN.currentChannelMode = mode;
setMode(i,EEReadMode(i));
}
startup = FALSE;
//printModes();
}
BOOL SetChanVal(BYTE pin,INT32 bval, float time){
BYTE mode = GetChannelMode(pin);
//println_I("Setting channel pos\n\tchan: ");p_int_I(pin);print_I(" \n\tto val: ");p_int_I(bval);print_I("\n\tin time: ");p_fl_I(time);
switch (mode){
case IS_DI:
case IS_DO:
SetDIO(pin,bval);
break;
case IS_SERVO:
SetServoPos(pin,bval,time);
break;
case IS_PWM:
SetPWM(pin,bval);
break;
case IS_DC_MOTOR_VEL:
case IS_DC_MOTOR_DIR:
SetDCMotor(pin,bval);
break;
default:
return FALSE;
}
return TRUE;
}
