bool Servo_Handler_1(void*)
{
timer16_Sequence_t timer = _timer2;
if (Channel[timer] < 0) {
//Timers.periodicHz(10000000, Servo_Handler_0, 0, 0); // channel set to -1 indicated that refresh interval completed so reset the timer
} else {
if (SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true) {
digitalWrite(SERVO(timer,Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
}
}
Channel[timer]++; // increment to the next channel
if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
Timers.timer(timer)->setFrequency(usToHz(ticksToUs(SERVO(timer,Channel[timer]).ticks)));
if(SERVO(timer,Channel[timer]).Pin.isActive == true) { // check if activated
digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
}
}
else {
Timers.timer(timer)->setFrequency(usToHz(REFRESH_INTERVAL));
Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
}
return true;
}
static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA)
{
if( Channel[timer] < 0 )
*TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
else{
if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true )
{
servo_t &servo = SERVO(timer,Channel[timer]);
servo.line_driver->lineWrite(servo.Pin.nbr,LOW); // pulse this channel low if activated
}
}
Channel[timer]++; // increment to the next channel
if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
*OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
if(SERVO(timer,Channel[timer]).Pin.isActive == true) // check if activated
{
servo_t &servo = SERVO(timer,Channel[timer]);
servo.line_driver->lineWrite(servo.Pin.nbr,HIGH); // its an active channel so pulse it high
}
}
else {
// finished all channels so wait for the refresh period to expire before starting over
if( (unsigned)*TCNTn < (usToTicks(REFRESH_INTERVAL) + 4) ) // allow a few ticks to ensure the next OCR1A not missed
*OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
else
*OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed
Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
}
}
void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel)
{
// clear interrupt
tc->TC_CHANNEL[channel].TC_SR;
if (Channel[timer] < 0) {
tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // channel set to -1 indicated that refresh interval completed so reset the timer
} else {
if (SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true) {
digitalWrite(SERVO(timer,Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
}
}
Channel[timer]++; // increment to the next channel
if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer,Channel[timer]).ticks;
if(SERVO(timer,Channel[timer]).Pin.isActive == true) { // check if activated
digitalWrite(SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
}
}
else {
// finished all channels so wait for the refresh period to expire before starting over
if( (tc->TC_CHANNEL[channel].TC_CV) + 4 < usToTicks(REFRESH_INTERVAL) ) { // allow a few ticks to ensure the next OCR1A not missed
tc->TC_CHANNEL[channel].TC_RA = (unsigned int)usToTicks(REFRESH_INTERVAL);
}
else {
tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + 4; // at least REFRESH_INTERVAL has elapsed
}
Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
}
}
void handle_interrupts(int timer, volatile unsigned int *TMRn, volatile unsigned int *PR)
{
if( channel[timer] < 0 )
*TMRn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
else{
if( SERVO_INDEX(timer,channel[timer]) < ServoCount && SERVO(timer,channel[timer]).Pin.isActive == true )
digitalWrite( SERVO(timer,channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated
}
channel[timer]++; // increment to the next channel
if( SERVO_INDEX(timer,channel[timer]) < ServoCount && channel[timer] < SERVOS_PER_TIMER) {
*PR = *TMRn + SERVO(timer,channel[timer]).ticks;
if(SERVO(timer,channel[timer]).Pin.isActive == true) // check if activated
digitalWrite( SERVO(timer,channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
}
else {
*TMRn = channel[timer] * usToTicks(MAX_PULSE_WIDTH);
// finished all channels so wait for the refresh period to expire before starting over
if( (unsigned)*TMRn < (usToTicks(REFRESH_INTERVAL) + 4) ) // allow a few ticks to ensure the next OCR1A not missed
*PR = (unsigned int)usToTicks(REFRESH_INTERVAL);
else
*PR = *TMRn + 4; // at least REFRESH_INTERVAL has elapsed
channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
}
}
//************************************************************************
// Note: PIC32 timers (TMRn) reset to zero when they match PRn.
void handle_interrupts(int timer, volatile unsigned int *TMRn, volatile unsigned int *PRn)
{
static uint32_t AccumulatedTicks[3] = {0,0,0}; // Store the number of ticks since the first rising edge for this timer
// Test for invalid timer number
if (timer >= 3)
{
return;
}
// If this value is -1, then we have just finished with the time from the end of the last pulse
// and need to start with the first servo index on this timer again.
if ( channel[timer] < 0 )
{
AccumulatedTicks[timer] = 0; // Clear the accumulated time for this timer on first rising edge
}
else
{
// If this is not the first pulse, then set the old pin low
if ( SERVO_INDEX(timer,channel[timer]) < ServoCount && SERVO(timer,channel[timer]).Pin.isActive == true )
{
digitalWrite( SERVO(timer,channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
}
}
channel[timer]++; // increment to the next channel
// If we have not run out of channels (on this timer),
if ( SERVO_INDEX(timer,channel[timer]) < ServoCount && channel[timer] < SERVOS_PER_TIMER)
{
// Then set the time we want to fire next (the width for this channel)
*PRn = SERVO(timer,channel[timer]).ticks;
// Set this channel's pin high if its active
if (SERVO(timer,channel[timer]).Pin.isActive == true) // check if activated
{
digitalWrite( SERVO(timer,channel[timer]).Pin.nbr, HIGH); // its an active channel so pulse it high
}
AccumulatedTicks[timer] += *PRn; // Add the time we are about to spend on this channel
}
else
{
// Otherwise, finished all channels so next fire needs to be at REFRESH_INTERVAL - AccumulatedTicks
if (AccumulatedTicks[timer] < (usToTicks(REFRESH_INTERVAL) - 4))
{
*PRn = (unsigned int)(usToTicks(REFRESH_INTERVAL)) - AccumulatedTicks[timer];
}
else
{
*PRn = *TMRn + 4; // at least REFRESH_INTERVAL has elapsed
}
channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
}
}
static void handle_interrupts(timer16_Sequence_t timer)
{
if( Channel[timer] == 0 ){
if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true ){
digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // pulse this channel0 high if activated
}
}
else if( Channel[timer] >= 1 ){
if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true ){
digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // pulse this channel high if activated
}
if( SERVO_INDEX(timer,Channel[timer]-1) < ServoCount && SERVO(timer,Channel[timer]-1).Pin.isActive == true ){
digitalWrite( SERVO(timer,Channel[timer]-1).Pin.nbr,LOW); // pulse this channel low if activated
}
}
Channel[timer]++; // increment to the next channel
if( SERVO_INDEX(timer,Channel[timer]) < ServoCount+1 && Channel[timer] < SERVOS_PER_TIMER){
SRV_TDRxx = SERVO(timer,Channel[timer]).ticks;
totalTicks += SERVO(timer,Channel[timer]).ticks;
}
else{
// finished all channels so wait for the refresh period to expire before starting over
if( totalTicks + 4 < usToTicks(REFRESH_INTERVAL)){ // allow a few ticks to ensure the next OCR1A not missed
SRV_TDRxx = (unsigned int)usToTicks(REFRESH_INTERVAL);
}
else{
SRV_TDRxx = 4; // at least REFRESH_INTERVAL has elapsed
}
Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
totalTicks = 0;
}
}
static boolean isTimerActive(timer16_Sequence_t timer) {
// returns true if any servo is active on this timer
for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
if (SERVO(timer,channel).Pin.isActive)
return true;
}
return false;
}
static bool isTimerActive(timer16_Sequence_t timer)
{
// returns TRUE if any servo is active on this timer
for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
if(SERVO(timer,channel).Pin.isActive == TRUE)
return TRUE;
}
return FALSE;
}
static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA)
{
register servo_t *pservo;
if( Channel[timer] < 0 )
*TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
else{
pservo = &SERVO(timer,Channel[timer]);
if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && pservo->Pin.isActive == TRUE ) {
digitalWrite( pservo->Pin.nbr,LOW); // pulse this channel low if activated
// See if we are in a timed move, if so update the move for the next time through...
if (pservo->ticksDelta > 0) {
pservo->ticks +=pservo->ticksDelta;
if (pservo->ticks >= pservo->ticksNew) {
pservo->ticks = pservo->ticksNew;
pservo->ticksDelta = 0;
}
}
else if (pservo->ticksDelta < 0) {
pservo->ticks +=pservo->ticksDelta;
if (pservo->ticks <= pservo->ticksNew) {
pservo->ticks = pservo->ticksNew;
pservo->ticksDelta = 0;
}
}
}
}
Channel[timer]++; // increment to the next channel
if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
pservo = &SERVO(timer,Channel[timer]);
*OCRnA = *TCNTn + pservo->ticks;
if(pservo->Pin.isActive == TRUE) // check if activated
digitalWrite( pservo->Pin.nbr,HIGH); // its an active channel so pulse it high
}
else {
// finished all channels so wait for the refresh period to expire before starting over
if( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) ) // allow a few ticks to ensure the next OCR1A not missed
*OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
else
*OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed
Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
}
}
void ServoHandler(int timer)
{
if (currentServoIndex[timer] < 0) {
// Write compare register
_timer->CCMP = 0;
} else {
if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) {
digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, LOW); // pulse this channel low if activated
}
}
// Select the next servo controlled by this timer
currentServoIndex[timer]++;
if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && currentServoIndex[timer] < SERVOS_PER_TIMER) {
if (SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) { // check if activated
digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high
}
// Get the counter value
uint16_t tcCounterValue = 0; //_timer->CCMP;
_timer->CCMP = (uint16_t) (tcCounterValue + SERVO(timer, currentServoIndex[timer]).ticks);
}
else {
// finished all channels so wait for the refresh period to expire before starting over
// Get the counter value
uint16_t tcCounterValue = _timer->CCMP;
if (tcCounterValue + 4UL < usToTicks(REFRESH_INTERVAL)) { // allow a few ticks to ensure the next OCR1A not missed
_timer->CCMP = (uint16_t) usToTicks(REFRESH_INTERVAL);
}
else {
_timer->CCMP = (uint16_t) (tcCounterValue + 4UL); // at least REFRESH_INTERVAL has elapsed
}
currentServoIndex[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
}
/* Clear flag */
_timer->INTFLAGS = TCB_CAPT_bm;
}
void main (void) {
volatile uint32_t i = 0; /* Dummy idle counter */
uint8_t option;
initModesAndClock(); /* Initialize mode entries and system clock */
initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */
disableWatchdog(); /* Disable watchdog */
initPads(); /* Initialize pads used in example */
initADC(); /* Init. ADC for normal conversions but don't start yet*/
initCTU(); /* Configure desired CTU event(s) */
initEMIOS_0(); /* Initialize eMIOS channels as counter, SAIC, OPWM */
initEMIOS_0ch3(); /* Initialize eMIOS 0 channel 3 as OPWM and channel 2 as SAIC*/
initEMIOS_0ch0(); /* Initialize eMIOS 0 channel 0 as modulus counter*/
initEMIOS_0ch23(); /* Initialize eMIOS 0 channel 23 as modulus counter*/
initEMIOS_0ch4(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 4 as time base */
initEMIOS_0ch6(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 6 as time base */
initEMIOS_0ch7(); /* Initialize eMIOS 0 channel 1 as OPWM, ch 7 as time base */
init_LinFLEX_0_UART();
SIU.PCR[17].R = 0x0200; /* Program the drive enable pin of Right Motor as output*/
SIU.PCR[16].R = 0x0200; /* Program the drive enable pin of Left Motor as output*/
SIU.PGPDO[0].R = 0x00000000; /* Disable the motors */
/* Loop forever */
for (;;)
{
TransmitData("\n\r**The Freescale Cup**");
TransmitData("\n\r*********************");
TransmitData("\n\r1.Led\n\r");
TransmitData("2.Switch\n\r");
TransmitData("3.Servo\n\r");
TransmitData("4.Motor Left\n\r");
TransmitData("5.Motor Right\n\r");
TransmitData("6.Camera\n\r");
TransmitData("9.Camera 2");
TransmitData("7.Left Motor Current\n\r");
TransmitData("8.Right Motor Current");
TransmitData("\n\r**********************");
option = ReadData();
switch(option)
{
case '1':
LED();
break;
case '2':
SWITCH();
break;
case '3':
SERVO();
break;
case '4':
MOTOR_LEFT();
break;
case '5':
MOTOR_RIGHT();
break;
case '6':
CAMERA();
break;
case '7':
LEFT_MOTOR_CURRENT();
break;
case '8':
RIGHT_MOTOR_CURRENT();
break;
case '9':
CAMERA2();
break;
default:
break;
}
}
}
void main (void) {
initModesAndClock(); /* Initialize mode entries and system clock */
initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */
disableWatchdog(); /* Disable watchdog */
initPads(); /* Initialize pads used in example */
initADC(); /* Init. ADC for normal conversions but don't start yet*/
initCTU(); /* Configure desired CTU event(s) */
initEMIOS_0(); /* Initialize eMIOS channels as counter, SAIC, OPWM */
initEMIOS_0ch3(); /* Initialize eMIOS 0 channel 3 as OPWM and channel 2 as SAIC*/
initEMIOS_0ch0(); /* Initialize eMIOS 0 channel 0 as modulus counter*/
initEMIOS_0ch23(); /* Initialize eMIOS 0 channel 23 as modulus counter*/
initEMIOS_0ch4(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 4 as time base */
initEMIOS_0ch6(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 6 as time base */
initEMIOS_0ch7(); /* Initialize eMIOS 0 channel 1 as OPWM, ch 7 as time base */
//init_LinFLEX_0_UART();
SIU.PCR[17].R = 0x0200; /* Program the drive enable pin of Right Motor as output*/
SIU.PCR[16].R = 0x0200; /* Program the drive enable pin of Left Motor as output*/
SIU.PGPDO[0].R = 0x00000000; /* Disable the motors */
// Routines
DC_Motors_on();
//******************************** INFINITE LOOP ***********************************
for (;;)
{
// 0. DEBUGGING CODE
//option = ReadData();
//printlistall();
// 1. Clean all variables
//sensor_value_left =0;
//sensor_value_right =0 ;
// 2. Sense the line
//option = ReadData();
//CAMERA();
CAMERA_car();
// 3. Calculate Differential
for (i=0;i<127;i++) // one less than 128
{
diff_result[i] = (short int) (Result[i+1] - Result [i]); // dy.dx
}
// 4. Find maximum and minimum indices
max_val = diff_result[START]; // can be improved to centre + max_delta
min_val = diff_result[START];
max_I = START;
min_I = START;
for (i=START+1;i<END;i++) //0 skipped
{
if (max_val < diff_result[i])
{
max_val = diff_result[i];
max_I = i;
}
if (min_val > diff_result[i])
{
min_val = diff_result[i];
min_I = i;
}
}
// 6. MAIN STATE MACHINE ALGO
width = max_I - min_I;
if (width> LB_WIDTH && width < UB_WIDTH)
{
// Everything is assumed normal.
// IF there is a spike (very less probability)
// it should be allowed to process, it can't misguide the car.
// Find centre
if (min_I < L_BOUND)
{
SERVO (SERVO_CENTRE - SERVO_LIMIT );
continue;
}
if (max_I > R_BOUND)
{
SERVO (SERVO_CENTRE + SERVO_LIMIT );
continue;
}
centre = (max_I + min_I) /2;
}
else
{
continue;
}
if ((centre - prev_centre) > FILTER)
{
prev_centre = centre;
centre = centre * 0.20;
}
else
{
prev_centre = centre;
}
// 8. Calculate error
error = centre - 64 ;
pid_term = (int) ( kp * error );
// 9. Calculate PID term
if (pid_term > SERVO_LIMIT)
{
pid_term = SERVO_LIMIT;
}
else if (pid_term <-SERVO_LIMIT)
{
pid_term = -SERVO_LIMIT;
}
// 10. Feed the new value to servo motor
correction = SERVO_CENTRE + pid_term;
SERVO (correction);
}
}
void main (void) {
volatile uint32_t i = 0; /* Dummy idle counter */
uint8_t option;
initModesAndClock(); /* Initialize mode entries and system clock */
initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */
disableWatchdog(); /* Disable watchdog */
initPads(); /* Initialize pads used in example */
initADC(); /* Init. ADC for normal conversions but don't start yet*/
initCTU(); /* Configure desired CTU event(s) */
initEMIOS_0(); /* Initialize eMIOS channels as counter, SAIC, OPWM */
initEMIOS_0ch3(); /* Initialize eMIOS 0 channel 3 as OPWM and channel 2 as SAIC*/
initEMIOS_0ch0(); /* Initialize eMIOS 0 channel 0 as modulus counter*/
initEMIOS_0ch23(); /* Initialize eMIOS 0 channel 23 as modulus counter*/
initEMIOS_0ch4(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 4 as time base */
initEMIOS_0ch6(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 6 as time base */
initEMIOS_0ch7(); /* Initialize eMIOS 0 channel 1 as OPWM, ch 7 as time base */
init_LinFLEX_0_UART();
SIU.PCR[17].R = 0x0200; /* Program the drive enable pin of Right Motor as output*/
SIU.PCR[16].R = 0x0200; /* Program the drive enable pin of Left Motor as output*/
SIU.PGPDO[0].R = 0x00000000; /* Disable the motors */
// set switchs as inputs. ..
SIU.PCR[64].R = 0x0100; /* Program the drive enable pin of S1 (PE0) as input*/
SIU.PCR[65].R = 0x0100; /* Program the drive enable pin of S2 (PE1) as input*/
SIU.PCR[68].R = 0x0200; /* Program the drive enable pin of LED1 (PE4) as output*/
SIU.PCR[69].R = 0x0200; /* Program the drive enable pin of LED2 (PE5) as output*/
SIU.PCR[70].R = 0x0200; /* Program the drive enable pin of LED3 (PE6) as output*/
SIU.PCR[71].R = 0x0200; /* Program the drive enable pin of LED4 (PE7) as output*/
SIU.PGPDO[2].R |= 0x0f000000; /* Disable LEDs*/
/* Loop forever */
for (;;)
{
if((SIU.PGPDI[2].R & 0x80000000) != 0x80000000)
{
if (correction <= 1300)
{
correction = 1300;
}
else
{
correction -=10;
}
}
else if((SIU.PGPDI[2].R & 0x40000000) != 0x40000000)
{
if (correction >= 1900)
{
correction = 1900;
}
else
{
correction +=10;
}
}
option = ReadData();
if (option == '6')
{
CAMERA_simar();
}
SERVO (correction);
}
}
static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA)
{
if( Channel[timer] < 0 )
*TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
else{
if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true )
digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated
}
Channel[timer]++; // increment to the next channel
if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
// Extension for slowmove
if (SERVO(timer,Channel[timer]).speed) {
// Increment ticks by speed until we reach the target.
// When the target is reached, speed is set to 0 to disable that code.
if (SERVO(timer,Channel[timer]).target > SERVO(timer,Channel[timer]).ticks) {
SERVO(timer,Channel[timer]).ticks += SERVO(timer,Channel[timer]).speed;
if (SERVO(timer,Channel[timer]).target <= SERVO(timer,Channel[timer]).ticks) {
SERVO(timer,Channel[timer]).ticks = SERVO(timer,Channel[timer]).target;
SERVO(timer,Channel[timer]).speed = 0;
}
}
else {
SERVO(timer,Channel[timer]).ticks -= SERVO(timer,Channel[timer]).speed;
if (SERVO(timer,Channel[timer]).target >= SERVO(timer,Channel[timer]).ticks) {
SERVO(timer,Channel[timer]).ticks = SERVO(timer,Channel[timer]).target;
SERVO(timer,Channel[timer]).speed = 0;
}
}
}
// End of Extension for slowmove
//original
*OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
if(SERVO(timer,Channel[timer]).Pin.isActive == true) // check if activated
digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
}
else {
// finished all channels so wait for the refresh period to expire before starting over
if( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) ) // allow a few ticks to ensure the next OCR1A not missed
*OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
else
*OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed
Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
}
}
void main (void) {
initModesAndClock(); /* Initialize mode entries and system clock */
initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */
disableWatchdog(); /* Disable watchdog */
initPads(); /* Initialize pads used in example */
initADC(); /* Init. ADC for normal conversions but don't start yet*/
initCTU(); /* Configure desired CTU event(s) */
initEMIOS_0(); /* Initialize eMIOS channels as counter, SAIC, OPWM */
initEMIOS_0ch3(); /* Initialize eMIOS 0 channel 3 as OPWM and channel 2 as SAIC*/
initEMIOS_0ch0(); /* Initialize eMIOS 0 channel 0 as modulus counter*/
initEMIOS_0ch23(); /* Initialize eMIOS 0 channel 23 as modulus counter*/
initEMIOS_0ch4(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 4 as time base */
initEMIOS_0ch6(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 6 as time base */
initEMIOS_0ch7(); /* Initialize eMIOS 0 channel 1 as OPWM, ch 7 as time base */
init_LinFLEX_0_UART();
SIU.PCR[17].R = 0x0200; /* Program the drive enable pin of Right Motor as output*/
SIU.PCR[16].R = 0x0200; /* Program the drive enable pin of Left Motor as output*/
SIU.PGPDO[0].R = 0x00000000; /* Disable the motors */
// Routines
DC_Motors_on();
//******************************** INFINITE LOOP ***********************************
for (;;)
{
// 0. DEBUGGING CODE
option = ReadData();
printlistall();
// 1. Clean all variables
//sensor_value_left =0;
//sensor_value_right =0 ;
// 2. Sense the line
//option = ReadData();
//CAMERA();
CAMERA_simar();
// 3. Calculate Differential
for (i=0; i<127; i++) // one less that 128
{
diff_result[i] = (short int) (Result[i+1] - Result [i]); // dy.dx
}
// 4. Find maximum and minimum indices
max_val = diff_result[centre]; // can be improved to centre + max_delta
min_val = diff_result[centre];
max_I = centre;
min_I = centre;
for (i=centre-1; i>2; i--) //0 skipped
{
if (max_val < diff_result[i])
{
max_val = diff_result[i];
max_I = i;
}
if (min_val > diff_result[i])
{
min_val = diff_result[i];
min_I = i;
}
}
for (i = centre+1; i<126; i++)
{
if (max_val < diff_result[i])
{
max_val = diff_result[i];
max_I = i;
}
if (min_val > diff_result[i])
{
min_val = diff_result[i];
min_I = i;
}
}
// 6. MAIN STATE MACHINE ALGO
width = max_I - min_I;
if (width> LB_WIDTH && width < UB_WIDTH)
{
// Everything is assumed normal.
// IF there is a spike (very less probability)
// it should be allowed to process, it can't misguide the car.
}
else
{
// Width is not right. Check for any psuedo max/ mins. We are assuming that
// there must be atleast one pseudo in this case. As width is not right.
diff = max_val;
if( diff > MIN_FINGER) // M is not pseudo
{
// sure (according to assumption above) m is pseudo.
// Two possibilities when one is pseudo
if (max_I < LEFT_GUARD)
{
// Guard detected
min_I = 0;
}
else
{
// 5.5 DYNAMIC INTEGRATION TIME
/* if ((max_val+ (-min_val))/2 <50 )
{
int_time = int_time*1.5;
}
else if ((max_val+ (-min_val))/2 >90 )
{
int_time = int_time*0.85;
}
*/
// Means that there had been a spike
// skip the case
// Become cautious and increase integration time.
//continue;
}
}
else
{
// M is pseudo and m can also be pseudo
// To test m is pseudo or not.
diff = min_val;
if ( diff > MIN_FINGER)
{
// m is not pseudo
// only M is pseudo
if (min_I > RIGHT_GUARD)
{
// Guard detected
max_I = 128;
}
else
{
// Means that there had been a spike
// skip the case
// Become cautious and increase integration time.
//continue;
}
}
else
{
// 5.5 DYNAMIC INTEGRATION TIME
if ((max_val+ (-min_val))/2 <50 )
{
int_time = int_time*1.5;
}
else if ((max_val+ (-min_val))/2 >90 )
{
int_time = int_time*0.85;
}
// both are pseudo
// So either a ALL_BALCK (CROSS) or ALL_WHITE
// Skip the case
// Become cautious and increase integration time.
continue;
}
}
}
// 7. Find centre
centre = (max_I + min_I) /2;
// 7.5 Filter
if (centre-prev_centre >15 || centre-prev_centre <-15)
{
prev_centre = centre;
centre = centre*0.30;
}
else
{
prev_centre = centre;
}
// 8. Calculate error
error = centre - 64 ;
pid_term = (int) ( kp * error );
// 9. Calculate PID term
if (pid_term > SERVO_LIMIT)
{
pid_term = SERVO_LIMIT;
}
else if (pid_term <-SERVO_LIMIT)
{
pid_term = -SERVO_LIMIT;
}
// 10. Feed the new value to servo motor
correction = SERVO_CENTRE + pid_term;
SERVO (correction);
}
}
void main (void) {
initModesAndClock(); /* Initialize mode entries and system clock */
initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */
disableWatchdog(); /* Disable watchdog */
initPads(); /* Initialize pads used in example */
initADC(); /* Init. ADC for normal conversions but don't start yet*/
initCTU(); /* Configure desired CTU event(s) */
initEMIOS_0(); /* Initialize eMIOS channels as counter, SAIC, OPWM */
initEMIOS_0ch3(); /* Initialize eMIOS 0 channel 3 as OPWM and channel 2 as SAIC*/
initEMIOS_0ch0(); /* Initialize eMIOS 0 channel 0 as modulus counter*/
initEMIOS_0ch23(); /* Initialize eMIOS 0 channel 23 as modulus counter*/
initEMIOS_0ch4(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 4 as time base */
initEMIOS_0ch6(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 6 as time base */
initEMIOS_0ch7(); /* Initialize eMIOS 0 channel 1 as OPWM, ch 7 as time base */
//init_LinFLEX_0_UART();
SIU.PCR[17].R = 0x0200; /* Program the drive enable pin of Right Motor as output*/
SIU.PCR[16].R = 0x0200; /* Program the drive enable pin of Left Motor as output*/
SIU.PGPDO[0].R = 0x00000000; /* Disable the motors */
// Routines
DC_Motors_on();
//******************************** INFINITE LOOP ***********************************
for (;;)
{
// 1. Clean all variables
//sensor_value_left =0;
//sensor_value_right =0 ;
// 2. Sense the line
//option = ReadData();
//CAMERA();
CAMERA_simar();
// 3. Calculate SUMs
sum_left =0;
sum_right=0;
for (i=1; i<64 ; i++)
{
sum_left += Result[i];
sum_right += Result[i+64];
}
// 4. Find Difference
error = sum_left - sum_right;
// 8. Calculate pid
pid_term = (int) ( kp * error );
// 9. Calculate PID term
if (pid_term > SERVO_LIMIT)
{
pid_term = SERVO_LIMIT;
}
else if (pid_term <-SERVO_LIMIT)
{
pid_term = -SERVO_LIMIT;
}
// 10. Feed the new value to servo motor
correction = SERVO_CENTRE + pid_term;
SERVO (correction);
// 11. Debugging code
//option = ReadData();
//printserialsingned (error);
}
}
