void UserInit(void){
//setPrintStream(&USBPutArray);
setPrintLevelInfoPrint();
println_I("\n\nStarting PIC initialization");
//DelayMs(1000);
hardwareInit();
println_I("Hardware Init done");
ReleaseAVRReset();
CheckRev();
LoadEEstore();
LoadDefaultValues();
CartesianControllerInit();
InitPID();
UpdateAVRLED();
lockServos();
setPrintLevelInfoPrint();
BOOL brown = getEEBrownOutDetect();
setCoProcBrownOutMode(brown);
setBrownOutDetect(brown);
println_I("###Starting PIC In Debug Mode###\n");// All printfDEBUG functions do not need to be removed from code if debug is disabled
DelayMs(1000);
//setPrintLevelErrorPrint();
println_E("Error level printing");
println_W("Warning level printing");
println_I("Info level printing");
}
void UserInit(void) {
//setPrintStream(&USBPutArray);
clearPrint();
setPrintLevelInfoPrint();
println_I("Start PIC");
//DelayMs(1000);
hardwareInit();
//println_I("Hardware Init done");
InitializeDyIODataTableManager();
CheckRev();
LoadEEstore();
UpdateAVRLED();
lockServos();
setPrintLevelInfoPrint();
boolean brown = getEEBrownOutDetect() ? true:false;
setBrownOutDetect(brown);
//Data table needs to be synced before the PID can init properly
SyncDataTable();
InitPID();
//println_I("###Starting PIC In Debug Mode###\n"); // All printfDEBUG functions do not need to be removed from code if debug is disabled
//DelayMs(1000);
setPrintLevelWarningPrint();
//println_E("Error level printing");
//println_W("Warning level printing");
//println_I("Info level printing");
}
/****************************************************
* This function manages the control mode state and
* transitions, including necessary PID init, resets..
****************************************************/
void control_mode_manager(void)
{
switch(control_mode.state)
{
//////////////////////////////////////////////////////////////////////
// OFF MODE
case OFF_MODE : TRAJMotor1_f.enable = 0;
TRAJMotor2_f.enable = 0;
TRAJMotor3_f.enable = 0;
TRAJMotor1_f.active = 0;
TRAJMotor2_f.active = 0;
TRAJMotor3_f.active = 0;
TRAJMotor1_f.exec = 0;
TRAJMotor2_f.exec = 0;
TRAJMotor3_f.exec = 0;
TRAJMotor1_f.busy = 0;
TRAJMotor2_f.busy = 0;
TRAJMotor3_f.busy = 0;
rcurrent1 = 0;
rcurrent2 = 0;
rcurrent3 = 0;
rcurrent1_req = 0;
rcurrent2_req = 0;
rcurrent3_req = 0;
control_flags.current_loop_active = 0;
control_flags.pos_loop_active = 0;
home_f.state = 0;
// home_f.done = 0;
P1DC1 = FULL_DUTY;
P1DC2 = FULL_DUTY;
P2DC1 = FULL_DUTY;
// x_cart = 0.0;
// y_cart = 0.0;
// z_cart = 0.0;
// STATE TRANSITIONS
if(control_mode.ax_pos_mode_req)
{
control_mode.state = AX_POS_MODE;
}
// STATE TRANSITIONS
else if(control_mode.torque_mode_req)
{
control_mode.state = TORQUE_MODE;
}
else if(control_mode.cart_mode_req)
{
control_mode.state = CART_MODE;
}
else if(control_mode.track_mode_req)
{
control_mode.state = TRACK_MODE;
InitNLFilter2Fx(&Joint1NLFOut, &Joint1NLFStatus);
InitNLFilter2Fx(&Joint2NLFOut, &Joint2NLFStatus);
InitNLFilter2Fx(&Joint3NLFOut, &Joint3NLFStatus);
Joint1NLFStatus.qdRcommand = mposition1;
Joint1NLFStatus.qdRprev = mposition1;
Joint1NLFStatus.qdXint = mposition1;
Joint1NLFStatus.MODE = 1;
Joint2NLFStatus.qdRcommand = mposition2;
Joint2NLFStatus.qdRprev = mposition2;
Joint2NLFStatus.qdXint = mposition2;
Joint2NLFStatus.MODE = 1;
Joint3NLFStatus.qdRcommand = mposition3;
Joint3NLFStatus.qdRprev = mposition3;
Joint3NLFStatus.qdXint = mposition3;
Joint3NLFStatus.MODE = 1;
}
// IF there is ANY transition, RESETS PIDs
if(control_mode.trxs)
{
// RESET MOTOR FAULT FLAGS (first byte)
status_flags.dword = status_flags.dword & 0xFFFFFF00;
//RESETS PIDs
InitPID(&PIDCurrent1, &PIDCurrent1_f,-1);
InitPID(&PIDCurrent2, &PIDCurrent2_f,-1);
InitPID(&PIDCurrent3, &PIDCurrent3_f,-1);
InitPID(&PIDPos1, &PIDPos1_f,0);
InitPID(&PIDPos2, &PIDPos2_f,0);
InitPID(&PIDPos3, &PIDPos3_f,0);
control_mode.trxs = 0;
}
break;
/////////////////////////////////////////////////////////////////////
// TORQUE MODE 1
case TORQUE_MODE : control_flags.current_loop_active = 1;
// STATE TRANSITIONS
if(control_mode.off_mode_req)
{
control_mode.state = OFF_MODE;
control_mode.trxs = 0;
}
break;
/////////////////////////////////////////////////////////////////////
// AXIS POSITION MODE
case AX_POS_MODE : control_flags.current_loop_active = 1;
control_flags.pos_loop_active = 1;
//abilito i 3 motori
TRAJMotor1_f.enable = 1;
TRAJMotor2_f.enable = 1;
TRAJMotor3_f.enable = 1;
// STATE TRANSITIONS
if(control_mode.off_mode_req)
{
control_mode.state = OFF_MODE;
control_mode.trxs = 0;
}
break;
/////////////////////////////////////////////////////////////////////
// CART MODE
case CART_MODE : control_flags.current_loop_active = 1;
control_flags.pos_loop_active = 1;
TRAJMotor1_f.enable = 1;
TRAJMotor2_f.enable = 1;
TRAJMotor3_f.enable = 1;
// STATE TRANSITIONS
if(control_mode.off_mode_req)
{
control_mode.state = OFF_MODE;
control_mode.trxs = 0;
}
break;
/////////////////////////////////////////////////////////////////////
// TRACK MODE
case TRACK_MODE : control_flags.current_loop_active = 1;
control_flags.track_loop_active = 1;
// STATE TRANSITIONS
if(control_mode.off_mode_req)
{
control_mode.state = OFF_MODE;
control_mode.trxs = 0;
}
break;
/////////////////////////////////////////////////////////////////////
// ERROR!!!!
default : break;
}//end SWITCH
}// END control mode manager
/*******************************************************
* Update parameters function, called at init and when
* a parameter is modified through SACT protocol
********************************************************/
void update_params(void)
{
uint32_t templong;//intero lungo temporaneo
uint8_t tempshift;//scalamento temporaneo
///////////////////////////////////////////////////////////////////
// VARIABLES FOR RUN-TIME USE OF PARAMETERS
max_current = parameters_RAM[0];//la corrente massima è in memoria RAM
//encoder_counts_rev = (int32_t)parameters_RAM[19] << 2; // TAKE INTO ACCOUNT x4 QEI MODE
//direction_flags.word = parameters_RAM[21];//anche la direzione è in memoria RAM
//ROBOT DIMENSION [in meters]
lf = parameters_RAM[13]/1000.0; //control arm lenght
le = parameters_RAM[14]/1000.0; //forearm lenght
e = parameters_RAM[15]/1000.0; //effector apothema
f = parameters_RAM[16]/1000.0; //base apothema
//ROBOT LIMITS [in degrees]
sphJLim = parameters_RAM[17]; //spheric joint limit
posJLim = parameters_RAM[18]; //positive joint limit
negJLim = parameters_RAM[19]; //negative joint limit
//encoder parameters
encoder_ticks = (int32_t) parameters_RAM[25] * parameters_RAM[26] * 4;//10200
kvel = (int32_t) ((float) 1/parameters_RAM[25] * FCY * 60); //4422000
decdeg_to_ticks = encoder_ticks / 3600.0;
decdeg_to_ticks_int = (uint16_t)((3600L << 10)/encoder_counts_rev); // in 5.10 fixed point
///////////////////////////////////////////////////////////////////
// CONTROL LOOPS and TRAJ PLANNERS INIT
////INIT PID CURRENT 1
PIDCurrent1.qKp = parameters_RAM[5];
PIDCurrent1.qKi = parameters_RAM[6];
PIDCurrent1.qKd = parameters_RAM[7];
PIDCurrent1.qN = parameters_RAM[8]; // SHIFT FINAL RESULT >> qN
PIDCurrent1.qdOutMax = (int32_t)(FULL_DUTY << (PIDCurrent1.qN-1));
PIDCurrent1.qdOutMin = -(int32_t)(FULL_DUTY << (PIDCurrent1.qN-1));
InitPID(&PIDCurrent1, &PIDCurrent1_f,-1);
////INIT PID CURRENT 2
PIDCurrent2.qKp = parameters_RAM[5];
PIDCurrent2.qKi = parameters_RAM[6];
PIDCurrent2.qKd = parameters_RAM[7];
PIDCurrent2.qN = parameters_RAM[8]; // SHIFT FINAL RESULT >> qN
PIDCurrent2.qdOutMax = (int32_t)(FULL_DUTY << (PIDCurrent2.qN-1));
PIDCurrent2.qdOutMin = -(int32_t)(FULL_DUTY << (PIDCurrent2.qN-1));
InitPID(&PIDCurrent2, &PIDCurrent2_f,-1);
////INIT PID CURRENT 3
PIDCurrent3.qKp = parameters_RAM[5];
PIDCurrent3.qKi = parameters_RAM[6];
PIDCurrent3.qKd = parameters_RAM[7];
PIDCurrent3.qN = parameters_RAM[8]; // SHIFT FINAL RESULT >> qN
PIDCurrent3.qdOutMax = (int32_t)(FULL_DUTY << (PIDCurrent3.qN-1));
PIDCurrent3.qdOutMin = -(int32_t)(FULL_DUTY << (PIDCurrent3.qN-1));
InitPID(&PIDCurrent3, &PIDCurrent3_f,-1);
////INIT PID POS 1
PIDPos1.qKp = parameters_RAM[9];
PIDPos1.qKi = parameters_RAM[10];
PIDPos1.qKd = parameters_RAM[11];
PIDPos1.qN = parameters_RAM[12]; // SHIFT FINAL RESULT >> qN
PIDPos1.qdOutMax = ((int32_t)max_current << PIDPos1.qN);
PIDPos1.qdOutMin = -PIDPos1.qdOutMax;
InitPID(&PIDPos1, &PIDPos1_f,0);
////INIT PID POS 2
PIDPos2.qKp = parameters_RAM[9];
PIDPos2.qKi = parameters_RAM[10];
PIDPos2.qKd = parameters_RAM[11];
PIDPos2.qN = parameters_RAM[12]; // SHIFT FINAL RESULT >> qN
PIDPos2.qdOutMax = ((int32_t)max_current << PIDPos2.qN);
PIDPos2.qdOutMin = -PIDPos2.qdOutMax;
InitPID(&PIDPos2, &PIDPos2_f,0);
////INIT PID POS 3
PIDPos3.qKp = parameters_RAM[9];
PIDPos3.qKi = parameters_RAM[10];
PIDPos3.qKd = parameters_RAM[11];
PIDPos3.qN = parameters_RAM[12]; // SHIFT FINAL RESULT >> qN
PIDPos3.qdOutMax = ((int32_t)max_current << PIDPos3.qN);
PIDPos3.qdOutMin = -PIDPos3.qdOutMax;
InitPID(&PIDPos3, &PIDPos3_f,0);
////INIT TRAJ PLANNER 1
TRAJMotor1_f.enable = 0;
TRAJMotor1.qVLIM = parameters_RAM[1];
TRAJMotor1.qACC = parameters_RAM[2];
TRAJMotor1.qVELshift = parameters_RAM[3];
TRAJMotor1.qACCshift = parameters_RAM[4];
TRAJMotor1.qdPosition = parameters_RAM[25];
////INIT TRAJ PLANNER 2
TRAJMotor2_f.enable = 0;
TRAJMotor2.qVLIM = parameters_RAM[1];
TRAJMotor2.qACC = parameters_RAM[2];
TRAJMotor2.qVELshift = parameters_RAM[3];
TRAJMotor2.qACCshift = parameters_RAM[4];
TRAJMotor2.qdPosition = parameters_RAM[25];
////INIT TRAJ PLANNER 3
TRAJMotor3_f.enable = 0;
TRAJMotor3.qVLIM = parameters_RAM[1];
TRAJMotor3.qACC = parameters_RAM[2];
TRAJMotor3.qVELshift = parameters_RAM[3];
TRAJMotor3.qACCshift = parameters_RAM[4];
TRAJMotor3.qdPosition = parameters_RAM[25];
////INIT Limits for nonlinear filter
// vel max = (VLIM for PosTRAJ >> VEL SCALE Shift) * POS_LOOP_FREQ
templong = parameters_RAM[1] >> parameters_RAM[3];
NLF_vel_max = templong << POS_LOOP_FcSHIFT;
// acc max = ((ACC for PosTRAJ >> ACC SCALE Shift) * POS_LOOP_FREQ^2 ) >> VEL SCALE Shift
templong = parameters_RAM[2] >> parameters_RAM[4];
templong = templong << (POS_LOOP_FcSHIFT<<1) ;
templong = templong >> parameters_RAM[3];
// search best 2^N scaling approximation for (acc max)^-1
tempshift = 0;
while((1UL<<tempshift) <= templong)
tempshift++;
NLF_acc_max_shift = tempshift-2;
//INIT HOMING!!!!!!!!!!!!!!!!!!!!!!!!!
home_f.done = 0;
home.Velocity = parameters_RAM[1] / parameters_RAM[20];
home.position1 = -(int32_t)parameters_RAM[22];
home.position2 = -(int32_t)parameters_RAM[23];
home.position3 = -(int32_t)parameters_RAM[24];
home.offsetPosition = parameters_RAM[21];
//-------------------------------------
}
