task main(){
//--------------------INIT Code---------------------------//
ForwardDistReset((tMotor)rtMotor, (tMotor)ltMotor);
DirectionReset();
nMotorEncoder[blockthrower] = 0;
speedCmdZ1=0;
pathIdx=0;
delayatruecount=0;
int state=0;
Pid_Init1();
Pid_Init2();
//--------------------End INIT Code--------------------------//
waitForStart(); // Wait for the beginning of autonomous phase.
int iFrameCnt=0;
int timeLeft;
servo[irArm]=243;
while(true){
ClearTimer(T1);
hogCPU();
//--------------------Robot Code---------------------------//
long armEncoder = nMotorEncoder[blockthrower];
long robotDist = nMotorEncoder[rtMotor] + nMotorEncoder[ltMotor];
long robotDir = nMotorEncoder[ltMotor] - nMotorEncoder[rtMotor];
long distInches = robotDist/IN2CLK;
long dirDegrees = robotDir/27;
// Calculate the speed and direction commands
int speedCmd = ForwardDist(path[pathIdx][DIST_IDX], robotDist, path[pathIdx][SPD_IDX]);
int dirCmd = Direction(path[pathIdx][DIR_IDX], robotDir);
int armSpd = FlipperArm(armEncoder, armSetPos);
bool IRval;
//calculate when to increment path
if (abs(path[pathIdx][DIR_IDX] - dirDegrees) < 4 && abs(path[pathIdx][DIST_IDX] - distInches) < 3) pathIdx++;
// Calculate the IR value
IRval = Delayatrue(1, SensorValue[IR] == 5 || SensorValue[IR] == 6);
if (state==0)// State O Follow Path
{
if (distInches>28)
{
state=1;
}
}
if(state==1)// State 1 Swing out irArm
{
servo[irArm]=150;
if (distInches>34)
{
state=2;
}
}
if (state==2)// state 2 look for ir under box 1
{
if ( IRval||SensorValue[IR] == 3||SensorValue[IR] == 2)
{
state=12;
servo[irArm]=243;
}
else
{
state=3;
}
}
if (state==12)//follows path before flipping arm
{
//speedCmd=0;
if(distInches>44)
{
state = 8;
}
}
if (state==3)//state 3 look for box 2 and follow path
{
if (distInches>46)
{
state=4;
}
}
if (state==4)//state 4 look for ir under box 2
{
if ( IRval==true||SensorValue[IR] == 3)
{
state=13;
servo[irArm]=243;
}
else
{
state=15;
}
servo[irArm]=243;
}
if (state==13) // waits for distance before flipping
{
if(distInches>57)
{
state = 8;
}
}
if (state==15) // pulls servo arm out
{
if(distInches>57)
{
servo[irArm] = 80;
state =5;
}
}
if (state==5)//state 5 look for box 3 and follow path
{
if (distInches>66)//36
{
state=6;
}
}
if (state==6)// State 6 Look for ir under box 3
{
if ( IRval||SensorValue[IR] == 4||SensorValue[IR] == 3||SensorValue[IR] == 2)
{
state=14;
}
else
{
state=7;
}
servo[irArm]=243;
}
if (state==14)// waits distance before flipping arm
{
if(distInches>69)
state = 8;
}
if (state==7)// State 7 look for box 4
{
if (pathIdx == 3)//45
{
state=8;
}
}
if (state==8)// state 8 flip arm
{
servo[irArm]=243;
speedCmd=0;
dirCmd = 0;
armSetPos = 2300;
if (abs(armSetPos - armEncoder) <10)
{
state=9;
}
}
if (state==9)//state 9 lower arm
{
speedCmd = 0;
dirCmd = 0;
armSetPos = 0;
if (abs(armSetPos - armEncoder) < 400)
{
pathIdx=3;
state=10;
}
}
if(state==10)//state 10 follow path
{
}
//DebugInt("spd",speedCmd);
//DebugInt("dir",robotDir/DEG2CLK);
//DebugInt("dist",distInches);
//DebugInt("path",pathIdx);
//DebugInt("state",state);
DebugInt("irval",SensorValue[IR]);
// Calculate when to move to the next path index
int s=sizeof(path)/sizeof(path[0])-1;
if (pathIdx>s) pathIdx=s; // Protect the path index
// Ramp the command up
speedCmd = RateLimit(speedCmd, START_RATE,speedCmdZ1);
leftmotors=Pid1(speedCmd+dirCmd);
rightmotors=Pid2(speedCmd-dirCmd);
//rightmotors=speedCmd-dirCmd;
//leftmotors=speedCmd+dirCmd;
motor[rtBack]=rightmotors;
motor[rtMotor]=rightmotors;
motor[ltMotor]=leftmotors;
motor[ltBack]=leftmotors;
motor[blockthrower]=armSpd;
//DebugInt("rightmotors",rightmotors);
//DebugInt("leftmotors",leftmotors);
//DebugInt("rightencoder",nMotorEncoder[rtMotor]);
//DebugInt("leftencoder",nMotorEncoder[ltMotor]);
if (iFrameCnt==0)
writeDebugStreamLine("i,pthIdx,rbtDist,irval,spdCmd,IR,state, rightencoder, leftencoder");
writeDebugStreamLine("%3i,%3i,%4i,%4i,%3i,%3i,%3i, %4i, %4i",iFrameCnt,pathIdx,distInches,IRval,speedCmd,SensorValue[IR],state, nMotorEncoder[rtMotor], nMotorEncoder[ltMotor]);
//--------------------------Robot Code--------------------------//
// Wait for next itteration
iFrameCnt++;
timeLeft=FOREGROUND_MS-time1[T1];
releaseCPU();
wait1Msec(timeLeft);
}// While
}//Foreground
task Foreground(){
//servoChangeRate[servo2] = 2;
Pid_Init1();
Pid_Init2();
int timeLeft;
int state=0;
int iFrameCnt = 0;
int speedCmd = 0;
int dirCmd = 0;
servo[irArm]=105;
const tMUXSensor LEGOUS = msensor_S4_3;
while(true){
ClearTimer(T1);
hogCPU();
//--------------------Robot Code---------------------------//
long armEncoder = nMotorEncoder[blockthrower];
long robotDist = nMotorEncoder[rtMotor] + nMotorEncoder[ltMotor];
long robotDir = nMotorEncoder[ltMotor] - nMotorEncoder[rtMotor];
long distInches = robotDist/IN2CLK;
long dirDegrees = robotDir/DEG2CLK;
iFrameCnt++;
// Calculate the speed and direction commands
if(shortPathTrue == false)
{
speedCmd = ForwardDist(path[pathIdx][DIST_IDX], robotDist, path[pathIdx][SPD_IDX]);
dirCmd=Direction(path[pathIdx][DIR_IDX], robotDir);
}
else
{
speedCmd = ForwardDist(shortPath[pathIdx][DIST_IDX], robotDist, shortPath[pathIdx][SPD_IDX]);
dirCmd=Direction(shortPath[pathIdx][DIR_IDX], robotDir);
}
int armSpd = FlipperArm(armEncoder, armSetPos);
bool IRval;
bool SonarVal;
//calculate when to increment path
if (abs(path[pathIdx][DIR_IDX] - dirDegrees) < 6 && abs(path[pathIdx][DIST_IDX] - distInches) < 3) pathIdx++;
// State O Follow Path
if (state==0)
{
if (distInches>18)
{
state=1;
}
}
IRval = Delayatrue(1, SensorValue[IR] == 4 || SensorValue[IR] == 3);
// State 1 Look for IR Beacon
if (state==1)
{
speedCmd=10;
if ( IRval)
{
state=7;
}
else
{
state=2;
}
}
// State 2 Follow Path
if (state==2)
{
if (distInches>27)
{
state=3;
}
}
// State 3 Look for IR Beacon
if (state==3)
{
speedCmd=10;
if ( IRval==true)
{
state=7;
}
else
{
state=4;
}
}
// State 4 Follow Path
if (state==4)
{
if (distInches>49)//36
{
state=5;
}
}
// State 5 Look for IR Beacon
if (state==5)
{
speedCmd=10;
if ( IRval==true)
{
state=7;
}
else
{
state=6;
}
}
// State 6 Follow Path
if (state==6)
{
if (pathIdx == 1)//45
{
state=7;
}
}
if (state==7)// flip arm
{
speedCmd=0;
dirCmd = 0;
armSetPos = 1900;
if (abs(armSetPos - armEncoder) <20)
{
state=8;
}
servo[irArm]=243;
}
if (state==8)
{
speedCmd = 0;
dirCmd = 0;
armSetPos = 0;
if (abs(armSetPos) - abs(armEncoder) < 200)
{
state=9;
}
}
SonarVal = Delayatrue2(1, USreadDist(LEGOUS) > 40);
if (state==9)
{
if ((distInches>90) && (distInches<115))
{
if(SonarVal)
{
state=10;
}
else
{
state=11;
}
}
}
if(state==10)
{
shortPathTrue=true;
state=11;
}
if(state==11)
{
}
/*
DebugInt("spd",speedCmd);
DebugInt("dir",robotDir/DEG2CLK);
DebugInt("sonarval",SonarVal);
DebugInt("path",pathIdx);
DebugInt("state",state);
DebugInt("dist",distInches);
DebugInt("ir", SensorValue[IR]);
*/
writeDebugStreamLine("%3i,%3i,%4i,%4i,%3i,%3i,%3i, %4i, %4i, %4i",iFrameCnt,pathIdx,distInches,IRval,speedCmd,SensorValue[IR],state, nMotorEncoder[rtMotor], nMotorEncoder[ltMotor], SonarVal);
// Calculate when to move to the next path index
int s=sizeof(path)/sizeof(path[0])-1;
DebugInt("siz",s);
if (pathIdx>s) pathIdx=s;
speedCmd = RateLimit(speedCmd, START_RATE,speedCmdZ1 );
// rightmotors=speedCmd-dirCmd;
// leftmotors=speedCmd+dirCmd;
leftmotors=Pid1(speedCmd+dirCmd);
rightmotors=Pid2(speedCmd-dirCmd);
motor[rtBack]=rightmotors;
motor[rtMotor]=rightmotors;
motor[ltMotor]=leftmotors;
motor[ltBack]=leftmotors;
motor[blockthrower]=armSpd;
DebugInt("rightmotors",rightmotors);
DebugInt("leftmotors",leftmotors);
//--------------------------Robot Code--------------------------//
// Wait for next itteration
timeLeft=FOREGROUND_MS-time1[T1];
releaseCPU();
wait1Msec(timeLeft);
}// While
}//Foreground
int main(int argc, char** argv)
{
unsigned int test;
Settings();
/* ******************************************************************************************************** */
/* ******************************************************************************************************** */
/* Inizializzazione da EEPROM */
/* Recupero i dati salvati in EEPROM ed eseguo un controllo di correttezza su di essi, questo serve */
/* principalmente per il default dopo la programmazione */
/* ******************************************************************************************************** */
/* ******************************************************************************************************** */
InitializationEEPROM();
/* ******************************************************************************************************** */
/* ******************************************************************************************************** */
/* Inizializzazione variabili */
/* */
/* ******************************************************************************************************** */
/* ******************************************************************************************************** */
LED1 = LED_OFF;
LED2 = LED_OFF;
MotorControlEnable(MOTORE1, MOTOR_DEACTIVE);
MotorControlEnable(MOTORE2, MOTOR_DEACTIVE);
DmaBuffer = 0;
VarModbus[INDICE_ENC1_TICK] = 0;
VarModbus[INDICE_ENC2_TICK] = 0;
VarModbus[INDICE_ENC1_PERIOD] = 0;
VarModbus[INDICE_ENC2_PERIOD] = 0;
VarModbus[INDICE_ENC1_SPEED] = 0;
VarModbus[INDICE_ENC2_SPEED] = 0;
//Motore1.L_WheelSpeed = 0;
//Motore2.L_WheelSpeed = 0;
VarModbus[INDICE_STATUSBIT1] = 0;
//VarModbus[INDICE_STATUSBIT1] &= ~(FLG_STATUSBI1_JOYMODE); // Al reset disattivo la modalità JoyStick
//VarModbus[INDICE_STATUSBIT1] &= ~(FLG_STATUSBI1_PID_EN); // Al reset disattivo la modalità PID
VarModbus[INDICE_STATUSBIT1] |= FLG_STATUSBI1_PID_EN; // Al reset attivo la modalità PID
OLD_INDICE_STATUSBIT1 = 0; // Se parto con il PID abilitato deve valere 0.
//OLD_INDICE_STATUSBIT1 = 1; // Se parto con il PWM abilitato deve valere 1.
VarModbus[INDICE_STATUSBIT1] |= FLG_STATUSBI1_COMWATCHDOG; // Al reset attivo il WatchDog sulla comunicazione
//VarModbus[INDICE_STATUSBIT1] &= ~(FLG_STATUSBI1_COMWATCHDOG); // Al reset disattivo il WatchDog sulla comunicazione
VarModbus[INDICE_STATUSBIT1] |= FLG_STATUSBI1_EEPROM_SAVE_EN; // Al reset attivo il Salvataggio automatico dei parametri in EEPROM
//VarModbus[INDICE_STATUSBIT1] &= ~(FLG_STATUSBI1_EEPROM_SAVE_EN); // Al reset disattivo il Salvataggio automatico dei parametri in EEPROM
VarModbus[INDICE_FLAG_TARATURA] = 0;
//VarModbus[INDICE_PWM_CH1] = MOTORE_STOP; // Motori fermi all'accensione
//VarModbus[INDICE_PWM_CH2] = MOTORE_STOP; // Motori fermi all'accensione
VarModbus[INDICE_PWM_CH1] = MOTORE_STOP_PID; // Motori fermi all'accensione
VarModbus[INDICE_PWM_CH2] = MOTORE_STOP_PID; // Motori fermi all'accensione
InitMotorStructure();
/* ******************************************************************************************************** */
/* ******************************************************************************************************** */
/* ******************************************************************************************************** */
/* Inizializzo i timer SW */
Timer1mSec.T_flag = FALSE;
Timer1mSec.T_initial_value = 1; // 1 = 1mSec
Timer1mSec.T_count_time = Timer1mSec.T_initial_value;
Timer10mSec.T_flag = FALSE;
Timer10mSec.T_initial_value = 10; // 10 = 10mSec
Timer10mSec.T_count_time = Timer10mSec.T_initial_value;
/* Inizializzazione porte seriali */
InizializzaSeriale(PORT_COM1);
InizializzaSeriale(PORT_COM2);
InitADC();
MotorControlEnable(MOTORE1, MOTOR_ACTIVE);
MotorControlEnable(MOTORE2, MOTOR_ACTIVE);
// TEST SEGNALAZIONI LED, sarà da eseguire dopo il controllo dei relativi FLAG...
SetLedErrorCode(&Led1Segnalazione, LED_ERRORCODE_05_CHECKERRORIOK, 1, SEGNALAZIONELED_TON, SEGNALAZIONELED_TOFF, SEGNALAZIONELED_TPAUSE);
SetLedErrorCode(&Led2Segnalazione, LED_POWERON_05_POWERTEST, 1, SEGNALAZIONELED_TON * 2, SEGNALAZIONELED_TOFF * 2, SEGNALAZIONELED_TPAUSE);
test = INTCON1;
ISR_Settings(); // Configures and enables ISRs
// /* DEBUG */
// SetpointRPM_M1 = Motore1.FL_Costante_Conversione_Vlin_to_Vang * ((float)((int)(100)));
// SetpointRPM_M2 = Motore2.FL_Costante_Conversione_Vlin_to_Vang * ((float)((int)(0)));
// if(!Motore1.UC_Fail) PID1.Setpoint = (long)(SetpointRPM_M1);
// if(!Motore2.UC_Fail) PID2.Setpoint = (long)(SetpointRPM_M2);
// /* ***** */
while (1)
{ // ---------------------- Gestione protocollo ModBus ---------------------- //
ModbusRoutine(PORT_COM1);
ModbusRoutine(PORT_COM2);
// ----------------- PID and speed calculation every 1ms ----------------- //
if (PID1_CALC_FLAG) Pid1(); // Ogni 1mSec ricalcola il prescaler, avvia un ciclo
if (PID2_CALC_FLAG) Pid2(); // di lettura dell'IC e esegue il PID sul dato prec.
// ---------------------- Task eseguito ogni 1mSec ---------------------- //
if (Timer1mSec.T_flag)
{
Timer1mSec.T_flag = FALSE;
GestioneWatchdog(); // GESTIONE WATCHDOG COMUNICAZIONE
GestioneSicurezzaMotore(); // Gestisco situazioni di FAIL dei motori
}
// ---------------------- Task eseguito ogni 10mSec ---------------------- //
if (Timer10mSec.T_flag)
{
Timer10mSec.T_flag = FALSE;
// GestioneWatchdog(); // GESTIONE WATCHDOG COMUNICAZIONE
// GestioneSicurezzaMotore(); // Gestisco situazioni di FAIL dei motori
GestioneLed1ErrorCode(&Led1Segnalazione);
GestioneLed2ErrorCode(&Led2Segnalazione);
}
GestioneAllarmi();
GestioneSetpoint();
AggiornaDatiVelocita();
AggiornaVariabiliModbus();
}
return (EXIT_SUCCESS);
}
int main(int argc, char** argv)
{
// Initialise le microcontroleur
Settings();
DelayN1ms(30);
// Initialise l'UART
InitTextIO();
// Initialise les interruptions
ISR_Settings();
initAsservissement();
initOdometrie();
// Configure tous les moteurs à l'arret
OC1RS = 0;
OC2RS = 0;
OC3RS = 0;
OC4RS = 0;
// Active les modules pour les PWM
OC1CONbits.OCM = 0b110;
OC2CONbits.OCM = 0b110;
OC3CONbits.OCM = 0b110;
OC4CONbits.OCM = 0b110;
uartNextOut = 0;
uartCommande = 0;
Cycle1 = 0;
Cycle2 = 0;
CYCLE1_FLAG = 0;
CYCLE2_FLAG = 0;
// Temporisation de 1 seconde
RtTimer = 10;
// Boucle Principale
while(1)
{
if(PID_CALC_FLAG)
{
Pid1();
Pid2();
compteurVitesse ++;
if(compteurVitesse == 10)
{
Vitesse[L] = VitesseCpteur[L]/400; // 400 = 10 échantillons x 40 pour le 1/40 mm/s => Le résultat est en mm/s
Vitesse[R] = VitesseCpteur[R]/400;
compteurVitesse = 0;
VitesseCpteur[L] = 0;
VitesseCpteur[R] = 0;
}
PID_CALC_FLAG = 0;
}
if(CYCLE1_FLAG)
{
Odometrie();
if(ordreEnCours != DEBUG)
{
Orientation();
}
CYCLE1_FLAG=0;
}
if(CYCLE2_FLAG)
{
Navigation();
CYCLE2_FLAG=0;
}
// blink LED
if (RtTimer <= 0)
{
RtTimer = 10;
LED_BLINK = !LED_BLINK;
}
if( uartCommande )
{
GererCommande();
}
}
return (EXIT_SUCCESS);
}