void CheckRev(void) {
LoadCorePacket(&downstreamPacketTemp);
downstreamPacketTemp.use.head.Method = BOWLER_GET;
downstreamPacketTemp.use.head.RPC = GetRPCValue("_rev");
downstreamPacketTemp.use.head.DataLegnth = 4;
SendPacketToCoProc(&downstreamPacketTemp);
if ((downstreamPacketTemp.use.data[0] == MAJOR_REV)
&& (downstreamPacketTemp.use.data[1] == MINOR_REV)
&& (downstreamPacketTemp.use.data[2] == FIRMWARE_VERSION)) {
SetColor(0, 0, 1);
} else {
//SetColor(1, 0, 0);
println_I("Rev. Check Failed! AVR:");
p_int_I(downstreamPacketTemp.use.data[0]);
print_I(".");
p_int_I(downstreamPacketTemp.use.data[1]);
print_I(".");
p_int_I(downstreamPacketTemp.use.data[2]);
print_I(" PIC:");
p_int_I(MAJOR_REV);
print_I(".");
p_int_I(MINOR_REV);
print_I(".");
p_int_I(FIRMWARE_VERSION);
}
}
float runPdVelocityFromPointer(PD_VEL* vel, float currentState,float KP, float KD){
float currentTime = getMs();
float timeMsDiff = (currentTime -vel->lastTime);
float timeDiff = timeMsDiff/1000;
float posDiff=currentState -vel->lastPosition;
float currentVelocity = posDiff/timeDiff;
//float velocityDiff = currentVelocity-vel->lastVelocity;
float velocityDiff=0;
float proportional = currentVelocity-vel->unitsPerSeCond;
float set = (proportional*KP)+(velocityDiff*KD)*timeMsDiff;
vel->currentOutputVel-=(set);
if (vel->currentOutputVel>200){
vel->currentOutputVel=200;
}else if(vel->currentOutputVel<-200){
vel->currentOutputVel=-200;
}
println_I("\t Velocity: set= ");p_fl_I(vel->unitsPerSeCond );print_I(" ticks/seCond" );
println_I("\t current state= ");p_fl_I(currentState );print_I(" ticks" );
println_I("\t last state= ");p_fl_I(vel->lastPosition );print_I(" ticks" );
println_I("\t position diff= ");p_fl_I(posDiff );print_I(" ticks" );
println_I("\t MS diff= ");p_fl_I(timeMsDiff );
println_I("\t current= ");p_fl_I(currentVelocity );print_I(" ticks/seCond" );
println_I("\t Velocity offset= ");p_fl_I(set );
println_I("\t Velocity set= ");p_fl_I(vel->currentOutputVel );
//cleanup
vel->lastPosition=currentState;
vel->lastVelocity=currentVelocity;
vel->lastTime=currentTime;
return vel->currentOutputVel;
}
BYTE setXYZ(float x, float y, float z,float ms){
updateCurrentPositions();
float t0=0,t1=0,t2=0;
if(hwMap.iK_callback( x, y, z, &t0, &t1, &t2)==0){
println_I("New target angles t1=");p_fl_I(t0);print_I(" t2=");p_fl_I(t1);print_I(" t3=");p_fl_I(t2);
setLinkAngle(0,t0,ms);
setLinkAngle(1,t1,ms);
setLinkAngle(2,t2,ms);
}else{
println_E("Interpolate failed, can't reach: x=");p_fl_E(x);print_E(" y=");p_fl_E(y);print_E(" z=");p_fl_E(z);
}
}
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);
}
}
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);
}
}
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);
}
}
int resetPositionMine(int group, int current){
println_I("Resetting PID Local ");p_int_I(group);print_I(" to ");p_int_I(current);print_I(" from ");p_fl_I(getPositionMine(group));
if(group<numPidMotors){
setCurrentValue(group, current);
}else{
resetHeater(group, current);
}
return getPositionMine(group);
}
void setOutputMine(int group, float v){
if( dyPid[group].outputChannel==DYPID_NON_USED||
((pidGroups[group].Enabled == FALSE) && (vel[group].enabled==FALSE)))
return;
Print_Level l = getPrintLevel();
setPrintLevelNoPrint();
int val = (int)(v);
//BYTE center = getBcsIoDataTable()[dyPid[group].outputChannel].PIN.currentConfiguration;
if(dyPid[group].outputMode == IS_SERVO){
val += 128;
if (val>254)
val=254;
if(val<0)
val=0;
}else if(dyPid[group].outputMode == IS_DO){
if(val>0)
val=1;
else
val=0;
}else{
val += 128;
if (val>255)
val=255;
if(val<0)
val=0;
}
int set = (int)val;
if (dyPid[group].outVal==set){
//if(!(RunEvery(&force[chan->channel])>0))
return;
}else{
print_I(" Setting PID output, was ");p_int_I(dyPid[group].outVal);print_I(" is now: ");p_int_I(set);print_I(" on DyIO chan: ");p_int_I(dyPid[group].outputChannel);print_I(", ");
}
dyPid[group].outVal=set;
println_I("PID setting output for group: ");p_int_I(group);
SetChannelValueCoProc(dyPid[group].outputChannel,dyPid[group].outVal);
setPrintLevel(l);
}
void CheckSwitches(void){
Print_Level l = getPrintLevel();
setPrintLevelInfoPrint();
switched=0;
volt = GetRawVoltage();
BOOL up = FALSE;
BYTE reg = isRegulated_0();
if (bankState[0] != reg ){
bankState[0]=reg;
up=TRUE;
}
reg = isRegulated_1();
if (bankState[1] != reg){
bankState[1] = reg;
up=TRUE;
}
if ((lastVolt>RawVoltageMin) && (volt<RawVoltageMin)){
up=TRUE;
lastVolt = volt;
}
if ((lastVolt<RawVoltageMin) && (volt>RawVoltageMin)){
up=TRUE;
lastVolt = volt;
}
if(up){
println_I("\nVoltage on raw: \t");
p_fl_I(volt);
println_I("Voltage on bank0: \t");
p_fl_I(GetRail0Voltage());
println_I("Voltage on bank1:\t");
p_fl_I(GetRail1Voltage());
println_I("Pushing upstream Power Packet bank 0: ");p_int_I(bankState[0]);print_I(" bank 1: ");p_int_I(bankState[1]);
println_I("Power Code 0: ");p_int_I(GetRawVoltageCode(0));
println_I("Power Code 1 : ");p_int_I(GetRawVoltageCode(1));
println_I("Raw: ");p_fl_I(GetRawVoltage());
UpstreamPushPowerChange();
}
setPrintLevel(l);
}
void printSortedData(){
int x;
print_I("Servo Data \r\n[ ");
for(x=0;x<dataTableSize;x++){
p_int_I(positionTemp[x]);print_I(" , ");
}
print_I(" ] ");
print_I("\r\n[ ");
for(x=0;x<dataTableSize;x++){
p_int_I(sort[x]);print_I(" , ");
}
print_I(" ] ");
print_I("\r\n[ ");
for(x=0;x<dataTableSize;x++){
p_int_I(positionTemp[sort[x]]);print_I(" , ");
}
print_I(" ] ");
}
float setLinkAngle(int index, float value, float ms){
int localIndex=linkToHWIndex(index);
float v = value/getLinkScale(index);
if( index ==0||
index ==1||
index ==2
){
// if(v>1650){
// println_E("Upper Capped link ");p_int_E(index);print_E(", attempted: ");p_fl_E(value);
// v=1650;
// }
// if(v<-6500){
// v=-6500;
// println_E("Lower Capped link ");p_int_E(index);print_E(", attempted: ");p_fl_E(value);
// }
}
println_I("Setting position from cartesian controller ");p_int_I(index);print_I(" to ");p_fl_I(v);
return SetPIDTimed(localIndex,v,ms);
}
void ProcessAsyncData(BowlerPacket * Packet){
//println_I("**Got Async Packet**");
//printPacket(Packet,INFO_PRINT);
Print_Level l = getPrintLevel();
setPrintLevelInfoPrint();
if (Packet->use.head.RPC==GCHV){
BYTE pin = Packet->use.data[0];
BYTE mode = GetChannelMode(pin);
if(mode == IS_ANALOG_IN ){
UINT16_UNION ana;
ana.byte.SB = Packet->use.data[1];
ana.byte.LB = Packet->use.data[2];
//ADC_val[pin-8]=ana.Val;
if(ana.Val>=0 && ana.Val<1024)
SetValFromAsync(pin,ana.Val);//asyncData[pin].currentVal=ana.Val;
println_I("***Setting analog value: ");p_int_I(pin);print_I(", ");p_int_I(ana.Val);
}
else if((mode == IS_DI) || (mode == IS_COUNTER_INPUT_HOME)|| (mode == IS_COUNTER_OUTPUT_HOME) || mode == IS_SERVO){
//DIG_val[pin]=Packet->use.data[1];
SetValFromAsync(pin,Packet->use.data[1]);//asyncData[pin].currentVal=Packet->use.data[1];
println_I("***Setting digital value: ");p_int_I(pin);print_I(", ");p_int_I(Packet->use.data[1]);//printStream(DIG_val,NUM_PINS);
}else {
if(IsAsync(pin)){
println_I("Sending async packet, not digital or analog");
PutBowlerPacket(Packet);
}
}
}else if (Packet->use.head.RPC==AASN){
int i;
for(i=0;i<8;i++){
BYTE pin = i+8;
BYTE mode = GetChannelMode(pin);
if(mode == IS_ANALOG_IN ){
UINT16_UNION ana;
ana.byte.SB = Packet->use.data[i*2];
ana.byte.LB = Packet->use.data[(i*2)+1];
//ADC_val[pin-8]=ana.Val
if(ana.Val>=0 && ana.Val<1024);
SetValFromAsync(pin,ana.Val);//asyncData[pin].currentVal=ana.Val;
}
}
}else if (Packet->use.head.RPC==DASN){
int i;
for(i=0;i<GetNumberOfIOChannels();i++){
BYTE mode = GetChannelMode(i);
if((mode == IS_DI) || (mode == IS_COUNTER_INPUT_HOME)|| (mode == IS_COUNTER_OUTPUT_HOME)|| (mode == IS_SERVO)){
SetValFromAsync(i,Packet->use.data[i]);//asyncData[i].currentVal=Packet->use.data[i];
}
}
//println_I("***Setting All Digital value: ");
}if (Packet->use.head.RPC==GetRPCValue("gacv")){
int i;
int val;
for(i=0;i<GetNumberOfIOChannels();i++){
val = get32bit(Packet, i*4);
if(getBcsIoDataTable()[i].PIN.asyncDataCurrentVal!=val){
println_I("Data on Pin ");p_int_I(i);print_I(" to val ");p_int_I(val);
SetValFromAsync(i,val);//
}
}
}else{
println_W("***Async packet not UNKNOWN***");
printPacket(Packet,WARN_PRINT);
}
// println_I("***Setting All value: [");
// int i;
// for(i=0;i<NUM_PINS;i++){
// p_int_I(asyncData[i].currentVal);print_I(" ");
// }
// print_I("]");
setPrintLevel(l);
}
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((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();
}
}
BOOL setMode(BYTE pin,BYTE mode){
println_I("Setting Mode: ");printMode(mode,INFO_PRINT);print_I(" on: ");p_int_I(pin);
BYTE currentMode = GetChannelMode(pin);
ClearCounter(pin);
StopSPI(pin);
clearPPM(pin);
print_I(" \tHardware Cleared");
switch (mode){
case IS_SERVO:
if(((pin < 12) && (isRegulated_0() == 0)) || ((pin >= 12) && (isRegulated_1()== 0)) ){
print_I("|Mode is now servo");
break;
}else{
if(getBrownOutDetect()){
print_I(" Servo Mode could not be set, voltage invalid");
return FALSE;
}else{
print_I(" Servo Mode set|");
break;
}
}
break;
case IS_SPI_MOSI:
case IS_SPI_MISO:
case IS_SPI_SCK:
if( pinHasFunction(pin, mode) != FALSE){
print_I("|Mode is now SPI");
InitSPI();
break;
}else{
return FALSE;
}
break;
case IS_COUNTER_INPUT_INT:
case IS_COUNTER_INPUT_DIR:
case IS_COUNTER_INPUT_HOME:
if(pinHasFunction(pin, mode) != FALSE){
print_I("|Mode is now Counter Input");
StartCounterInput(pin);
break;
}else{
print_I(", Counter Input not availible");
return FALSE;
}
break;
case IS_COUNTER_OUTPUT_INT:
case IS_COUNTER_OUTPUT_DIR:
case IS_COUNTER_OUTPUT_HOME:
if(pinHasFunction(pin, mode) != FALSE){
print_I("|Mode is now Counter Output");
StartCounterOutput(pin);
break;
}else{
print_I(", Counter Output not availible");
return FALSE;
}
break;
case IS_PPM_IN:
println_I("Setting up PPM...");
startPPM(pin);
break;
}
print_I(" \tMode set");
return TRUE;
}
BOOL setMode(BYTE pin,BYTE mode){
ClearPinState(pin);
println_I("Pin :");p_int_I(pin);print_I(" is mode: ");printMode(mode,INFO_PRINT);
//BYTE pwm,dir;
if (mode == NO_CHANGE){
return TRUE;
}
switch (mode){
case HIGH_IMPEDANCE:
ClearPinState(pin);
// Return here so as not to save this state to the eeprom
return TRUE;
case IS_UART_TX:
case IS_UART_RX:
if(pin == 17 || pin == 16){
configPinMode(16,IS_UART_TX,OUTPUT,ON);
configPinMode(17,IS_UART_RX,INPUT,ON);
InitUART();
return TRUE;
}
break;
case IS_SPI_MOSI:
case IS_SPI_MISO:
case IS_SPI_SCK:
if(pin == 0 || pin == 1||pin == 2 ){
configPinMode(0,IS_SPI_SCK,INPUT,ON);
configPinMode(1,IS_SPI_MISO,INPUT,ON);
configPinMode(2,IS_SPI_MOSI,INPUT,ON);
return TRUE;
}
break;
case IS_ANALOG_IN:
configPinMode(pin,mode,INPUT,OFF);
if(InitADC(pin)){
return TRUE;
}
break;
case IS_PWM:
if(InitPWM(pin)){
return TRUE;
}
return FALSE;
case IS_DC_MOTOR_VEL:
case IS_DC_MOTOR_DIR:
if(InitDCMotor(pin)){
return TRUE;
}
return FALSE;
case IS_SERVO:
InitServo(pin);
configPinMode(pin,mode,OUTPUT,OFF);
return TRUE;
case IS_DO:
configPinMode(pin,mode,OUTPUT,OFF);
return TRUE;
case IS_DI:
case IS_PPM_IN:
case IS_COUNTER_OUTPUT_INT:
case IS_COUNTER_OUTPUT_DIR:
case IS_COUNTER_OUTPUT_HOME:
case IS_COUNTER_INPUT_INT:
case IS_COUNTER_INPUT_DIR:
case IS_COUNTER_INPUT_HOME:
configPinMode(pin,mode,INPUT,ON);
return TRUE;
default:
configPinMode(pin,mode,INPUT,ON);
return TRUE;
}
return FALSE;
}
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
}