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 advancedBowlerExample() {
/**
* User code must implement a few functions needed by the stack internally
* This Platform specific code can be stored in the Platform directory if it is universal for all
* implementations on the given Platform.
* float getMs(void) Returns the current milliseconds since the application started
* StartCritical() Starts a critical protected section of code
* EndCritical() Ends the critical section and returns to normal operation
*/
setPrintLevelInfoPrint();// enable the stack specific printer. If you wish to use this feature putCharDebug(char c) needs to be defined
printf("\r\nStarting Sample Program\r\n");
int pngtmp = GetRPCValue("_png");
if(pngtmp != _PNG) {
printf("\r\nFAIL Expected: ");
prHEX32(_PNG,INFO_PRINT);
printf(" got: ");
prHEX32(pngtmp,INFO_PRINT);
return;
}
/**
* First we are going to put together dummy array of packet data. These are examples of a _png receive and a custom response.
* These would not exist in your implementation but would come in from the physical layer
*/
BowlerPacket myPngPacket;
myPngPacket.use.head.RPC = GetRPCValue("apid");
myPngPacket.use.head.MessageID = 2;// Specify namespace 2 for the collision detect
myPngPacket.use.head.Method = BOWLER_GET;
myPngPacket.use.head.DataLegnth=4;
FixPacket(&myPngPacket);// Set up the stack content
BowlerPacket myNamespacTestPacket;
myNamespacTestPacket.use.head.RPC = GetRPCValue("rtst");
myNamespacTestPacket.use.head.Method = BOWLER_GET;
myNamespacTestPacket.use.data[0] = 37;
myNamespacTestPacket.use.head.DataLegnth=4+1;
FixPacket(&myNamespacTestPacket);// Set up the stack content
/**
* Now we begin to set up the stack features. The first step is to set up the FIFO to receive the data coming in asynchronously
*
*/
BYTE privateRXCom[sizeof(BowlerPacket)];//make the buffer at least big enough to hold one full sized packet
BYTE_FIFO_STORAGE store;//this is the FIFO data storage struct. All interactions with the circular buffer will go through this.
/**
* Next we initialize the buffer
*/
InitByteFifo(&store,// the pointer to the storage struct
privateRXCom,//pointer the the buffer
sizeof(privateRXCom));//the size of the buffer
Bowler_Init();// Start the Bowler stack
/**
* Now we are going to regester what namespaces we implement with the framework
*/
NAMESPACE_LIST * tmp =getBcsPidNamespace();
addNamespaceToList(tmp);
tmp = getBcsTestNamespace();
addNamespaceToList(tmp);
printf("\r\n# of namespaces declared= %i",getNumberOfNamespaces());
int i=0;
for(i=0; i<getNumberOfNamespaces(); i++) {
NAMESPACE_LIST * nsPtr = getNamespaceAtIndex(i);
printf("\r\nNamespace %s at index %i",nsPtr->namespaceString,i);
}
/**
* Now we load the buffer with the the packet that we "Received"
* This step would come in from the physical layer, usually on
* an interrupt on a mcu.
*/
for (i=0; i<GetPacketLegnth(&myPngPacket); i++) {
BYTE err;// this is a stack error storage byte. See Bowler/FIFO.h for response codes
BYTE b= myPngPacket.stream[i];// This would be a new byte from the physical layer
FifoAddByte(&store, b, &err);// This helper function adds the byte to the storage buffer and manages the read write pointers.
}
/**
* Next we load the new namespace packet
*/
i=0;
for (i=0; i<GetPacketLegnth(&myNamespacTestPacket); i++) {
BYTE err;// this is a stack error storage byte. See Bowler/FIFO.h for response codes
BYTE b= myNamespacTestPacket.stream[i];// This would be a new byte from the physical layer
FifoAddByte(&store, b, &err);// This helper function adds the byte to the storage buffer and manages the read write pointers.
}
printf("\r\nData loaded into packet\r\n");
/**
* We have now loaded a packet into the storage struct 'store'
* All the while we can be polling the storage struct for a new packet
*/
BowlerPacket myLocalPacket; // Declare a packet struct to catch the parsed packet from the asynchronous storage buffer
while(getNumBytes(&store)>0) {
while(Bowler_Server_Static(&myLocalPacket,// pointer to the local packet into which to store the parsed packet
&store// storage struct from which the packet will be checked and parsed.
) == FALSE) { // Returns true when a packet is found and pulled out of the buffer
// wait because there is no packet yet
}
/**
* At this point the packet has been parsed and pulled out of the buffer
* The Static server will have also called the call backs to pars the packet, so
* the response should be loaded up to send back
*/
int packetLength = GetPacketLegnth(&myLocalPacket); // helper function to get packet length
printf("\r\nPreparing to send:\r\n");
printPacket(&myLocalPacket, INFO_PRINT);
printf("\r\nSending Packet Data back out: [ ");
for(i=0; i< packetLength; i++) {
//This would be sending to the physical layer. For this example we are just printing out the data
printf(" %i ",myLocalPacket.stream[i]);
}
printf(" ] \r\n");
}
}
void hardwareInit() {
// Configure the device for maximum performance but do not change the PBDIV
// Given the options, this function will change the flash wait states, RAM
// wait state and enable prefetch cache but will not change the PBDIV.
// The PBDIV value is already set via the pragma FPBDIV option above..
SYSTEMConfig((80000000L), SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);
SYSTEMConfigPerformance(80000000);
CHECONbits.PREFEN = 0;
int j = 0, i = 0;
for (i = 0; i < 6; i++) {
MyMAC.v[i] = MY_MAC_ADDRESS[i];
}
StartCritical();
println_I("MAC");
enableFlashStorage(true);
FlashGetMac(MyMAC.v);
for (i = 0; i < 6; i++) {
macStr[j++] = GetHighNib(MyMAC.v[i]);
macStr[j++] = GetLowNib(MyMAC.v[i]);
}
macStr[12] = 0;
//println_I("MAC address is =");
print_I(macStr);
Pic32_Bowler_HAL_Init();
usb_CDC_Serial_Init(dev, macStr, 0x04D8, 0x3742);
InitLEDS();
SetColor(0, 0, 1);
mInitSwitch();
//AVR Reset pin
InitAVR_RST();
HoldAVRReset();
//AVR must be running before pin states can be synced in the pin initialization
ReleaseAVRReset();
//Starts co-proc uart
initCoProcCom();
InitPinFunction();
//Must initialize IO before hardware
LoadDefaultValues();
//println_W("Pin States");
SyncModes();
//println_I("Modes synced, initializing channels");
initAdvancedAsync();
//println_I("Adding IO Namespace");
addNamespaceToList( get_bcsIoNamespace());
//println_I("Adding IO.Setmode Namespace");
addNamespaceToList(get_bcsIoSetmodeNamespace());
//println_I("Adding DyIO Namespace");
addNamespaceToList(get_neuronRoboticsDyIONamespace());
//println_I("Adding PID Namespace");
addNamespaceToList( getBcsPidNamespace());
//println_I("Adding DyIO PID Namespace");
addNamespaceToList( get_bcsPidDypidNamespace());
//println_I("Adding Safe Namespace");
addNamespaceToList((NAMESPACE_LIST *) get_bcsSafeNamespace());
Init_FLAG_BUSY_ASYNC();
//InitCTS_RTS_HO();
//ConfigUARTOpenCollector();
ConfigUARTRXTristate();
//Starts Timer 3
InitCounterPins();
InitADC();
//SetFwRev(rev);
GetName(Name);
if(Name[0]==0xff){
for(i=0;i<17;i++){
Name[i]=defaultName[i] ;
}
SetName(Name);
GetName(Name);
}
if (!GetLockCode(LockCode)){
for(i=0;i<4;i++){
LockCode[i] = defaultlock[i];
}
SetLockCode(LockCode);
}
EndCritical();
initBluetooth();
if(!hasBluetooth()){
Pic32UARTSetBaud( 115200 );
}
boolean defaultmac=true;
for (i = 0; (i < 6) && defaultmac; i++) {
if(MyMAC.v[i] != MY_MAC_ADDRESS[i]){
defaultmac = false;
}
}
if(defaultmac){
srand((unsigned) GetRawVoltage());// random seed from the air
MyMAC.v[3] = MINOR_REV;
MyMAC.v[4] = FIRMWARE_VERSION;
MyMAC.v[5] = rand() % 255;
FlashSetMac(MyMAC.v);
U1CON = 0x0000;
DelayMs(100);
Reset();
}
}
void hardwareInit(){
StartCritical();
println_I("Getting MAC from flash");
FlashGetMac(MyMAC.v);
char macStr[13];
int j=0,i=0;
for (i=0;i<6;i++){
macStr[j++]=GetHighNib(MyMAC.v[i]);
macStr[j++]=GetLowNib(MyMAC.v[i]);
}
macStr[12]=0;
println_I("MAC address is =");
print_I(macStr);
#if defined(ROBOSUB_DEMO)
//char * dev = "AHD Wave";
#else
char * dev = "DyIO v.3";
#endif
Pic32_Bowler_HAL_Init();
usb_CDC_Serial_Init(dev,macStr,0x04D8,0x3742);
mInitSwitch();
for (i=0;i<6;i++){
MyMAC.v[i]= MY_MAC_ADDRESS[i];
}
//Must initialize IO before hardware
InitPins();
println_I("Adding IO Namespace");
addNamespaceToList((NAMESPACE_LIST * )get_bcsIoNamespace());
println_I("Adding IO.Setmode Namespace");
addNamespaceToList((NAMESPACE_LIST * )get_bcsIoSetmodeNamespace());
println_I("Adding DyIO Namespace");
addNamespaceToList((NAMESPACE_LIST * )get_neuronRoboticsDyIONamespace());
println_I("Adding PID Namespace");
addNamespaceToList((NAMESPACE_LIST * )getBcsPidNamespace());
println_I("Adding DyIO PID Namespace");
addNamespaceToList((NAMESPACE_LIST * )get_bcsPidDypidNamespace());
println_I("Adding Safe Namespace");
addNamespaceToList((NAMESPACE_LIST * )get_bcsSafeNamespace());
Init_FLAG_BUSY_ASYNC();
//InitCTS_RTS_HO();
//AVR Reset pin
InitAVR_RST();
HoldAVRReset();
//ConfigUARTOpenCollector();
ConfigUARTRXTristate();
InitLEDS();
SetColor(0,0,1);
//Starts Timer 3
InitCounterPins();
InitADC();
BYTE rev [] = {MAJOR_REV,MINOR_REV,FIRMWARE_VERSION};
FlashSetFwRev(rev);
//Starts co-proc uart
initCoProcCom();
EndCritical();
INTEnableSystemMultiVectoredInt();
// initBluetooth();
// if(!hasBluetooth()){
// Pic32UARTSetBaud( 115200 );
// }
}
void hardwareInit(){
// Configure the device for maximum performance but do not change the PBDIV
// Given the options, this function will change the flash wait states, RAM
// wait state and enable prefetch cache but will not change the PBDIV.
// The PBDIV value is already set via the pragma FPBDIV option above..
SYSTEMConfig(SYS_FREQ, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);
SYSTEMConfigPerformance(80000000);
(_TRISF5)=INPUT; // for the reset sw
ATX_DISENABLE();
CloseTimer2();
Pic32_Bowler_HAL_Init();
Bowler_Init();
clearPrint();
println_I("\n\n\nStarting PIC initialization");
FlashGetMac(MyMAC.v);
DelayMs(2000);//This si to prevent runaway during programming
// enable driven to 3.3v on uart 1
mPORTDOpenDrainClose(BIT_3); mPORTFOpenDrainClose(BIT_5);
char macStr[13];
for (i=0;i<6;i++){
macStr[j++]=GetHighNib(MyMAC.v[i]);
macStr[j++]=GetLowNib(MyMAC.v[i]);
}
macStr[12]=0;
println_I("MAC address is =");
print_I(macStr);
char * dev = "BowlerDevice";
println_I(dev);
//This Method calls INTEnableSystemMultiVectoredInt();
usb_CDC_Serial_Init(dev,macStr,0x04D8,0x0001);
addNamespaceToList((NAMESPACE_LIST *)getBcsCartesianNamespace());
addNamespaceToList((NAMESPACE_LIST *)getBcsPidNamespace());
ATX_ENABLE(); // Turn on ATX Supply, Must be called before talking to the Encoders!!
println_I("Starting Encoders");
initializeEncoders();// Power supply must be turned on first
println_I("Starting Heater");
initializeHeater();
println_I("Starting Servos");
initServos();
#if !defined(NO_PID)
println_I("Starting PID");
initPIDLocal();
#endif
initializeCartesianController();
DelayMs(100);
if( GetPIDCalibrateionState(linkToHWIndex(0))!=CALIBRARTION_DONE&&
GetPIDCalibrateionState(linkToHWIndex(1))!=CALIBRARTION_DONE&&
GetPIDCalibrateionState(linkToHWIndex(2))!=CALIBRARTION_DONE
){
for(i=0;i<numPidMotors;i++){
SetPIDEnabled(i,true) ;
}
println_I("Running calibration for kinematics axis");
runPidHysterisisCalibration(linkToHWIndex(0));
runPidHysterisisCalibration(linkToHWIndex(1));
runPidHysterisisCalibration(linkToHWIndex(2));
DelayMs(100);//wait for ISR to fire and update all values
for(i=0;i<3;i++){
setPIDConstants(linkToHWIndex(i),.2,.1,0);
}
OnPidConfigure(0);
}else{
println_W("Axis are already calibrated");
}
pid.MsTime=getMs();
//startHomingLinks();
disableSerialComs(true) ;
(_TRISB0)=1;
SetColor(1,1,1);
HEATER_2_TRIS = OUTPUT;
//HEATER_1_TRIS = OUTPUT; // Causes one of the axies to crawl downward in bursts when enabled and on...
//HEATER_0_TRIS = OUTPUT; // causes device to twitc. These are touched by the USB stack somehow..... and as the reset button
}
