// -------------------------------------------
// called from main()
void pidSetup()
{
int i;
for(i = 0; i < NUM_PIDS; i++){
initPIDObjPos( &(pidObjs[i]), DEFAULT_KP, DEFAULT_KI, DEFAULT_KD, DEFAULT_KAW, DEFAULT_FF);
}
initPIDVelProfile();
SetupTimer1(); // main interrupt used for leg motor PID
// returns pointer to queue with 8 move entries
moveq = mqInit(8);
idleMove = malloc(sizeof(moveCmdStruct));
idleMove->inputL = 0;
idleMove->inputR = 0;
idleMove->duration = 0;
currentMove = idleMove;
manualMove = malloc(sizeof(moveCmdStruct));
manualMove->inputL = 0;
manualMove->inputR = 0;
manualMove->duration = 0;
lastMoveTime = 0;
// initialize PID structures before starting Timer1
pidSetInput(0,0,0);
pidSetInput(1,0,0);
EnableIntT1; // turn on pid interrupts
calibBatteryOffset(100);
}
static void MQTTS_Disconnect(void)
{
// Reset buffers
vMQTTS.head = 0;
vMQTTS.tail = 0;
vMQTTS.fHead = 0;
vMQTTS.fTail = 0;
// (Re)Initialize Memory manager
mqInit();
#ifndef GATEWAY
vMQTTS.Status = MQTTS_STATUS_SEARCHGW;
#else // Gateway
vMQTTS.Status = MQTTS_STATUS_OFFLINE;
#endif // Gateway
vMQTTS.pfCnt = POLL_TMR_FREQ - 1;
vMQTTS.Tretry = 0;
vMQTTS.Nretry = MQTTS_DEF_NRETRY;
vMQTTS.MsgID = 0;
vMQTTS.ReturnCode = 0xFF;
CleanOD();
#ifdef GATEWAY
vMQTTS.GatewayID = rf_GetNodeID();
#endif // GATEWAY
}
int main(void)
{
// Initialise System Hardware
INIT_SYSTEM();
// Initialise Memory manager
mqInit();
// Initialise Object's Dictionary
InitOD();
// Initialise PHY's
PHY1_Init();
#ifdef PHY2_ADDR_t
PHY2_Init();
#endif // PHY2_ADDR_t
// Initialize MQTTSN
MQTTSN_Init();
// Initialise optional components
#ifdef LEDsInit
LEDsInit();
#endif // LEDsInit
#ifdef DIAG_USED
DIAG_Init();
#endif // USE_DIAG
SystemTickCnt = 0;
StartSheduler();
while(1)
{
if(SystemTickCnt)
{
SystemTickCnt--;
OD_Poll();
MQTTSN_Poll();
#ifdef DIAG_USED
DIAG_Poll();
#endif // USE_DIAG
}
MQ_t * pBuf;
pBuf = PHY1_Get();
if(pBuf != NULL)
{
mqttsn_parser_phy1(pBuf);
}
#ifdef PHY2_Get
pBuf = PHY2_Get();
if(pBuf != NULL)
{
mqttsn_parser_phy2(pBuf);
}
#endif // PHY2_Get
}
}
//Main hall effect sensor setup, called from main()
void hallSetup() {
//Init of PID controller objects
int i;
for (i = 0; i < NUM_HALL_PIDS; i++) {
hallInitPIDObjPos(&(hallPIDObjs[i]), DEFAULT_HALL_KP, DEFAULT_HALL_KI,
DEFAULT_HALL_KD, DEFAULT_HALL_KAW, DEFAULT_HALL_FF);
hallPIDObjs[i].minVal = 0;
hallPIDObjs[i].satValNeg = 0;
hallPIDObjs[i].maxVal = FULLTHROT;
hallPIDObjs[i].satValPos = SATTHROT;
}
// Controller to PWM channel correspondance
hallOutputChannels[0] = MC_CHANNEL_PWM1;
hallOutputChannels[1] = MC_CHANNEL_PWM2;
//Init for velocity profile objects
hallInitPIDVelProfile();
//System setup
SetupTimer1();
SetupTimer2(); // used for leg hall effect sensors
SetupInputCapture(); // setup input capture for hall effect sensors
int retval;
retval = sysServiceInstallT1(hallServiceRoutine);
// returns pointer to queue with 8 move entries
hallMoveq = mqInit(8);
hallIdleMove = malloc(sizeof (moveCmdStruct));
hallIdleMove->inputL = 0;
hallIdleMove->inputR = 0;
hallIdleMove->duration = 0;
hallCurrentMove = hallIdleMove;
hallManualMove = malloc(sizeof (moveCmdStruct));
hallManualMove->inputL = 0;
hallManualMove->inputR = 0;
hallManualMove->duration = 0;
lastMoveTime = 0;
// initialize PID structures before starting Timer1
hallPIDSetInput(0, 0, 0);
hallPIDSetInput(1, 0, 0);
for (i = 0; i < NUM_HALL_PIDS; i++) {
hallbemfLast[i] = 0;
hallbemfHist[i][0] = 0;
hallbemfHist[i][1] = 0;
hallbemfHist[i][2] = 0;
}
}
int main()
{
SOCKET sockFD, newSockFD;
int portNo, cliLen;
struct sockaddr_in serv_addr, cli_addr;
sockFD = socket(AF_INET, SOCK_STREAM, 0);
if(sockFD < 0)
{
printf("Error opening a socket!\n");
exit(1);
}
portNo = DEFAULT_PORT;
serv_addr.sin_family = AF_INET;
serv_addr.sin_port = htons(portNo);
serv_addr.sin_addr.s_addr = htonl(INADDR_ANY); /* ip adress of machine server is running. Now obtained automatically. */
printf("INADDR_ANY: %d\n", INADDR_ANY);
if(bind(sockFD, (struct sockaddr*) &serv_addr, sizeof(serv_addr)) < 0)
{
printf("Error binding the socket!\n");
close(sockFD);
exit(1);
}
listen(sockFD, MAX_WAITING_CONS);
TMQueue* mainBuffer = mqInit();
while(true)
{
newSockFD = accept(sockFD, (struct sockaddr*) &cli_addr, (socklen_t*) &cliLen);
if(newSockFD < 0)
{
printf("Error on accepting socket!\n");
}
else
{
char connectedIP[INET_ADDRSTRLEN];
inet_ntop(AF_INET, &cli_addr.sin_addr, connectedIP, INET_ADDRSTRLEN);
printf("New connection accepted: %s\n", connectedIP);
int pid = fork();
if(pid < 0)
{
printf("Error on fork!\n");
}
else
{
if(pid == 0)
{
/* need to create and pass logged in users structure for this function and buffers */
processNewFork(newSockFD, (struct sockaddr_in*) &cli_addr, (TMainBuffer*) mainBuffer);
exit(0);
}
}
}
}
printf("Closing main socket...\n");
close(newSockFD);
return 0;
}
void legCtrlSetup() {
int i;
//Setup for PID controllers
for (i = 0; i < NUM_MOTOR_PIDS; i++) {
#ifdef PID_HARDWARE
//THe user is REQUIRED to set up these pointers before initializing
//the object, because the arrays are local to this module.
motor_pidObjs[i].dspPID.abcCoefficients =
motor_abcCoeffs[i];
motor_pidObjs[i].dspPID.controlHistory =
motor_controlHists[i];
#endif
pidInitPIDObj(&(motor_pidObjs[i]), LEG_DEFAULT_KP, LEG_DEFAULT_KI,
LEG_DEFAULT_KD, LEG_DEFAULT_KAW, LEG_DEFAULT_KFF);
//Set up max's and saturation values
motor_pidObjs[i].satValPos = SATTHROT;
motor_pidObjs[i].satValNeg = 0;
motor_pidObjs[i].maxVal = FULLTHROT;
motor_pidObjs[i].minVal = 0;
}
//Set which PWM output each PID Object will correspond to
legCtrlOutputChannels[0] = MC_CHANNEL_PWM1;
legCtrlOutputChannels[1] = MC_CHANNEL_PWM2;
SetupTimer1(); // Timer 1 @ 1 Khz
int retval;
retval = sysServiceInstallT1(legCtrlServiceRoutine);
//ADC_OffsetL = 1; //prevent divide by zero errors
//ADC_OffsetR = 1;
//Move Queue setup and initialization
moveq = mqInit(32);
idleMove = malloc(sizeof (moveCmdStruct));
idleMove->inputL = 0;
idleMove->inputR = 0;
idleMove->duration = 0;
idleMove->type = MOVE_SEG_IDLE;
idleMove->params[0] = 0;
idleMove->params[1] = 0;
idleMove->params[2] = 0;
currentMove = idleMove;
currentMoveStart = 0;
moveExpire = 0;
blinkCtr = 0;
inMotion = 0;
//Ensure controllers are reset to zero and turned off
//External function used here since it will zero out the state
pidSetInput(&(motor_pidObjs[0]), 0);
pidSetInput(&(motor_pidObjs[1]), 0);
motor_pidObjs[0].onoff = PID_OFF;
motor_pidObjs[1].onoff = PID_OFF;
//Set up filters and histories
for (i = 0; i < NUM_MOTOR_PIDS; i++) {
bemfLast[i] = 0;
bemfHist[i][0] = 0;
bemfHist[i][1] = 0;
bemfHist[i][2] = 0;
}
}