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; } } }