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;
}
Exemple #22
0
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);
}
Exemple #23
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;

}
Exemple #24
0
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;
}
Exemple #29
0
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;
}