int main (void) {
// SYSTEMConfigPerformance(40000000);
initTMR();
initLCD();
initADC();
initPWM();
int i;
while(1) {
if (IFS0bits.AD1IF == 1)
{
analogVal = ADC1BUF0;
IFS0bits.AD1IF = 0;
getAnalogVoltage(analogVal);
}
//moveCursorLCD(1,1);
//printCharLCD('c');
//getAnalogVoltage(analogVal);
}
}
void stateMachine(void)
{
/******************************************************************************
* INIT State
* Executes at boot up or when the processor is reset.
*
******************************************************************************/
if (state == STATE_INIT)
{
initPeripherals();
initInterrupts();
unsigned int checkRead;
checkRead = M24LC64FReadWord(CHECK_ADDRESS, M24LC64F_ADDRESS_0);
__delay32(EEPROM_DELAY*10);
if (checkRead)
{
homePos = 0;
M24LC64FWriteWord(HOME_ADDRESS, homePos ,M24LC64F_ADDRESS_0);
__delay32(EEPROM_DELAY*10);
M24LC64FWriteWord(CHECK_ADDRESS, homePos ,M24LC64F_ADDRESS_0);
__delay32(EEPROM_DELAY*10);
}
homePos = M24LC64FReadWord(HOME_ADDRESS, M24LC64F_ADDRESS_0); /* Retrieve the stored delay value */
//homePos = 0;
__delay32(EEPROM_DELAY*10);
motorPos = M24LC64FReadWord(MOTOR_POS_ADDRESS, M24LC64F_ADDRESS_0); /* Retrieve the stored Motor Position value */
//motorPos = 0;
__delay32(EEPROM_DELAY*10);
initPWM();
initADC();
initTMR();
indexMotor(homePos, MAX_STEPS); //MAX_STEPS is 250, moveMotor() steps 4 motor steps at a time.
}
/* End of INIT State*/
/******************************************************************************
* WARM_UP State
* As long as the warm up complete signal is not given the AFC will remain in
* this state.
******************************************************************************/
/*if (state == STATE_WARM_UP)
{
triggerINT = disableINT0();
//updateAnalogOut(homePos);
bufferFull = 0;
}*/
/******************************************************************************
* MAN State
* Executes if the user selected MAN using the proper hardware control signals
* This state enables the user to manually change configuration parameters and
* the position of the motor.
******************************************************************************/
if (state == STATE_MAN)
{
if (triggerINT || IFS0bits.INT0IF) /* Disabling the external interrupt INT0, only needed in AFC state */
{
triggerINT = disableINT0();
}
if (!MAN_DELAY_CTRL == 1) /* Check if we are setting Motor Position (= 1) or Delay (= 0) */
{
motorOrDelay = 0;
homePos = motorPos;
updateAnalogOut(homePos);
M24LC64FWriteWord(HOME_ADDRESS, homePos ,M24LC64F_ADDRESS_0);
}
else
{
motorOrDelay = 1;
updateAnalogOut(motorPos);
}
if (!MAN_DELAY_UP == 1)
{
__delay32(EEPROM_DELAY*10); /* Debouncing (50ms Delay) can be made faster for practical application */
if (motorOrDelay == 1)
{
unsigned int countTemp = 0;
count =0;
//do{
while(!MAN_DELAY_UP == 1)
{
//__delay32(EEPROM_DELAY*4);
setDir = FORWARD;
motorPos++;
moveMotor(setDir, TIMER_PERIOD2);
while(count==countTemp);
countTemp++;
updateAnalogOut(motorPos);
}
//M24LC64FWriteWord(MOTOR_POS_ADDRESS, motorPos, M24LC64F_ADDRESS_0);
//}while (count <500);
setDir = STOP;
moveMotor(setDir, TIMER_PERIOD2);
}
else
{
if (homePos >= MAX_STEPS)
homePos = MAX_STEPS;
else
homePos ++;
updateAnalogOut(homePos);
M24LC64FWriteWord(HOME_ADDRESS, homePos ,M24LC64F_ADDRESS_0);
}
}
if (!MAN_DELAY_DOWN == 1)
{
__delay32(EEPROM_DELAY*10); /* Debouncing (50ms Delay) can be made faster for practical application */
if (motorOrDelay == 1)
{
unsigned int countTemp = 0;
count =0;
//do{
while(!MAN_DELAY_DOWN == 1)
{
//__delay32(EEPROM_DELAY*4);
setDir = REVERSE;
motorPos--;
moveMotor(setDir, TIMER_PERIOD2);
while(count==countTemp);
countTemp++;
updateAnalogOut(motorPos);
}
//M24LC64FWriteWord(MOTOR_POS_ADDRESS, motorPos, M24LC64F_ADDRESS_0);
//}while (count <500);
setDir = STOP;
moveMotor(setDir, TIMER_PERIOD2);
}
else
{
if (homePos <= MIN_DELAY)
homePos = 0;
else
homePos --;
updateAnalogOut(homePos);
M24LC64FWriteWord(HOME_ADDRESS, homePos ,M24LC64F_ADDRESS_0);
}
}
}
/* End of MAN State*/
/******************************************************************************
* AFC State
* Executes if the user selected AFC using the proper hardware control signals
* This state locks all manual control and the AFC controls the motor position
******************************************************************************/
if (state == STATE_AFC)
{
if (!MAN_DELAY_UP == 1)
{
indexMotor(motorPos, MAX_STEPS);
}
if (!MAN_DELAY_DOWN == 1)
{
indexMotor(homePos, MAX_STEPS);
}
heatPerPulse = 0;
if (sampleTrigger)
{
sampleTrigger = 0;
prevReflectedPower = 0;
/* The ADC is triggered by a special comparison event of the PWM module */
ADCPC2bits.SWTRG5 = 1; /*Trigger ADC to convert AN11 (Heat Per Pulse input from PLC) */
while(!ADSTATbits.P5RDY);
heatPerPulse = ADCBUF11;
ADCPC0bits.SWTRG0 = 1; /*Trigger ADC to convert AN0 (Reflected port) */
while(!ADSTATbits.P0RDY);
prevReflectedPower = reflectedPower;
reflectedPower = ADCBUF0;
// delta = ADCBUF1;
ADSTATbits.P0RDY = 0; /* Clear the ADSTAT bits */
//ADSTATbits.P1RDY = 0;
ADSTATbits.P5RDY = 0;
/*__asm__ volatile ("clr B");
__asm__ volatile ("lac %0,B":"+r"(heatPerPulse));
__asm__ volatile ("sftac B,#16");
__asm__ volatile ("add A");*/
heatAccumulator += heatPerPulse;
/*No buffer*/
error = (int)(reflectedPower - prevReflectedPower);
/* Filtering using a FIFO buffer*/
/*errorArray[strPTR] = (int)(reflectedPower - prevReflectedPower);
strPTR++;
if (strPTR >= endPTR)
{
bufferFull = 1;
strPTR = 0;
}
if (bufferFull == 1)
{
int i;
for (i = 0; i < endPTR; i++)
{
error += errorArray[i];
}
error = error >> 5; //BUFFER_SIZE = 32 -> 2^5 = 32
}*/
/* else
{
int j;
for (j = 0; j < strPTR; j++)
{
error += errorArray[j];
}
error = error/strPTR;
}*/
triggerINT = enableINT0();
//calcError(); /* Calculate Error based on A/D results for reflected power*/
//moveMotor(setDir, motorStep);
//while (T2CONbits.TON);
}
if (thermalCounter == 50000) //Using the PWM special event interrupt to increment every TMR1 period match (~20us) 5000*20uS ~= 100mS
{
/*Reduce the accumulator by the cool rate coef.*/
heatAccumulator -= (heatAccumulator>>COOL_SHIFTS)*COOL_RATE;
}
/* Outer loop, calculates Thermal Drift effects and moves motor proactivley (Feedforward)*/
target = homePos + calcThermalError() + error;
moveMotorThermal();
}
int main(void) {
initTMR();
initSensor();
initMotor();
TRISButton = OUTPUT;
WEAKASSPULLUP = ENABLED;
TRISLEDR = OUTPUT;
TRISLEDF = OUTPUT;
TRISLEDL = OUTPUT;
while (1) {
switch (state) {
case idle:
stop();
if (button == PRESSED) state = debouncePress;
nextState = follow;
break;
case wait:
if (button == UNPRESSED) state = debounceRelease;
break;
case debouncePress:
delayUs(5000);
state = wait;
break;
case debounceRelease:
delayUs(5000);
state = nextState;
break;
case follow:
nextState = idle;
//delayUs(5000);
if (IFS0bits.AD1IF == 1) {
IFS0bits.AD1IF = 0;
front = getSensorFront();
left = getSensorLeft();
right = getSensorRight();
}
if ((right == ONTAPE || left == ONTAPE)) {
stop();
if (left == ONTAPE && front == OFFTAPE) alignLeft();
else if (right == ONTAPE && front == OFFTAPE) alignRight();
else if(right == ONTAPE && front == ONTAPE && left == ONTAPE) flip();
else forward();
}
else if (front == ONTAPE) {
forward();
}
/*else if(front == ONTAPE && (right == ONTAPE || left == ONTAPE)) {
stop();
if(turns < 3) turnRight();
else if (turns == 3) flip();
else turnLeft();
turns++;
}*/
if (button == PRESSED) state = debouncePress;
break;
}
}
}
