/* Start user code for adding. Do not edit comment generated here */
void packet_process(void)
{
int pwm; /* duty cycle for duty assignment mode */
int mode; /* assigned mode from master packet */
/* if ignore bit set, do nothing */
if (masterPacket[0] & 2)
return;
mode = masterPacket[0] & 12;
/* servo off mode */
if (mode == 0)
{
set_motor_output(0);
TRG = 0;
count = 0;
}
/* duty assignment mode */
if (mode == 4)
{
pwm = (double)((int8_t)masterPacket[3]) / 100 * 3100;
set_motor_output(pwm);
}
/* position control mode */
if (mode == 8)
{
pid_control();
}
/* else reservation mode, don't need to do anything */
/* if this is the time we are processing this packet, and if required send reply */
if (firstCall == 1 && masterPacket[1] & 1 == 1)
{
firstCall = 0;
bufferOut[0] = masterPacket[0];
bufferOut[2] = count >> 8;
bufferOut[3] = count;
bufferOut[4] = crc((((long)bufferIn[0]) << 24) +
(((long)bufferIn[1]) << 16) +
(((long)bufferIn[2]) << 8) +
((long) bufferIn[3]));*/
P1.5 = 1;
R_UART2_Send(bufferOut , 5);
/* when send finishes, it will toggle DE for receive */
}
void main (void)
{
long prevCounter = 0;
unsigned char tempTrigger = 0;
char tempStepPulses = 0;
long tempCurrentCounter = 0;
unsigned char currentADvalue = tempADSpeed;
long desiredCounter = 0;
long error = 0;
long prevError = 0;
long velocity = 0;
float integral = 0.0;
float derivative = 0.0;
long output = 0;
char testState = 0;
long testCounter = 0;
char homeFlag = 0;
enum { LOW = 0, HIGH = 1} homeDir = LOW;
/*************** SETUP INTERRUPTS ******************/
INTCONbits.GIEH = 0; // Temporarily disable global interrupts
INTCONbits.GIEL = 0; // Temporarily disable peripheral interrupts
INTCONbits.TMR0IE = 1; // Enable TMR0 Overflow interrupt
INTCONbits.INT0IE = 1; // Enable external interrupt on INT0/RB0
INTCONbits.RBIE = 1; // Enable Port B change interrupt
//INTCON2bits.RBPU = 0; // Enable Port B pull-ups
INTCON2bits.RBPU = 1; // Disable Port B pull-ups
INTCON2bits.INTEDG0 = 1; // Interrupt on rising edge for INT0
INTCON2bits.TMR0IP = 0; // Set TMR0 interrupt priority to Low
INTCON2bits.RBIP = 1; // Set Port B change interrupt to High
RCONbits.IPEN = 1; // Enable priority levels on interrupts
/****************************************************/
/***************** SETUP TMR0 ***********************/
//T0CONbits.TMR0ON = 0; // Temporarily disable TMR0 until the end of the setup;
T0CONbits.T08BIT = 1; // Use an 8-bit timer (1). Use a 16-bit timer (0).
T0CONbits.T0CS = 0; // Use internal clock cycles for the timer.
T0CONbits.PSA = 0; // Use prescalar to adjust overflow rate (0). Do not use prescalar (1).
/* Assign Prescalar */
T0CONbits.T0PS2 = 0;
T0CONbits.T0PS1 = 1;
T0CONbits.T0PS0 = 1;
/* This makes go slower */
//T0CONbits.T0PS2 = 1;
//T0CONbits.T0PS1 = 1;
//T0CONbits.T0PS0 = 1;
T0CONbits.TMR0ON = 1;
/***************************************************/
/************** SETUP I/O PINS *********************/
TRISBbits.TRISB4 = 1; // Encoder ChA input
TRISBbits.TRISB5 = 1; // Encoder ChB input
TRISBbits.TRISB6 = 0; // Set as output to avoid Port B interrupt on change
TRISBbits.TRISB7 = 0; // Set as output to avoid Port B interrupt on change
TRISBbits.TRISB0 = 1; // Step pulse input
TRISBbits.TRISB1 = 1; // Direction input
//TRISAbits.TRISA4 = 0; // Possible motor output pin RA4
TRISAbits.TRISA0 = 1; // Set RA0 as input for SPEED pin (analog input for A/D)
TRISAbits.TRISA1 = 0; // Set RA1 as output for diagnostic LED
//ADCON0bits.VCFG0 = 0; // Set A/D to External Vref(+/-) using 1 and 1
//ADCON0bits.VCFG1 = 0; // Set A/D to Vss and Vdd references using 0 and 0
/***************************************************/
/**************** SETUP A/D ************************/
/* Configure analog and digital pins */
ADCON1bits.PCFG0 = 1; // Set AN0 port as digital input (Home switch)
ADCON1bits.PCFG1 = 1; // RA1/AN1 diagnostic LED
ADCON1bits.PCFG2 = 1; // Reserved - test pad on AN2
ADCON1bits.PCFG3 = 1; // Reserved - test pad on AN3
ADCON1bits.PCFG4 = 1; // Set AN4/RB0 as a digital pin (Step pulse input)
ADCON1bits.PCFG5 = 1; // Set AN5/RB1 as a digital pin (Direction input)
ADCON1bits.PCFG6 = 1; // Set AN6/RB4 as a digital pin (ChA input)
//
// /* Select A/D input channel as AN0 */
// ADCON0bits.CHS0 = 0;
// ADCON0bits.CHS1 = 0;
// ADCON0bits.CHS2 = 0;
//
// /* Set A/D clock as Fosc/2 */
// ADCON2bits.ADCS0 = 0;
// ADCON2bits.ADCS1 = 0;
// ADCON2bits.ADCS2 = 0;
//
// ADCON2bits.ADFM = 0; // Left justify A/D conversion so only the 8 MSB are read later
//
// /* Turn on A/D module */
// ADCON0bits.ADON = 1;
/* Turn off A/D module */
ADCON0bits.ADON = 0;
/***************************************************/
/**************** SETUP PWM ************************/
/* Configure PWM pins P1A and P1B as inputs. */
TRISBbits.TRISB3 = 1;
TRISBbits.TRISB2 = 1;
/* Use PR2 to set the PWM period */
PR2 = 0xFF;
// Equation 15-1 PWM Period = [(PR2 + 1)] * 4 * TOSC * TMR2 Prescaler
// or about 1 / 490Hz
/* Set PWM Mode with P1A as a single modulated output */
CCP1CONbits.CCP1M0 = 0;
CCP1CONbits.CCP1M1 = 0;
CCP1CONbits.CCP1M2 = 1;
CCP1CONbits.CCP1M3 = 1;
CCP1CONbits.P1M0 = 0;
CCP1CONbits.P1M1 = 0;
/* LSB of the PWM duty cycle */
CCP1CONbits.DC1B1 = 0;
CCP1CONbits.DC1B0 = 0;
/* MSB of the PWM duty cycle */
CCPR1L = 0x00;
/* Configure TMR2 */
PIR1bits.TMR2IF = 0; // Clear interupt flag for TMR2 equal to PR2
T2CONbits.T2CKPS1 = 1; // Set prescaler to 16 to decrease PWM frequency
T2CONbits.T2CKPS0 = 1;
T2CONbits.TMR2ON = 1; // Turn on Timer2
/* Wait for the TMR2IF flag go high before procedding */
while (!(PIR1bits.TMR2IF))
{
int temp = 0;
}
/* Set the PWM pins P1A and P1B as outputs. */
TRISBbits.TRISB3 = 0;
TRISBbits.TRISB2 = 0;
ECCPASbits.ECCPASE = 0;
/*****************************************************/
/***************** SET OSC SPEED *********************/
//OSCCONbits.IRCF0 = 1;
//OSCCONbits.IRCF1 = 1;
//OSCCONbits.IRCF2 = 1;
OSCCONbits.IRCF0 = 1;
OSCCONbits.IRCF1 = 1;
OSCCONbits.IRCF2 = 1;
/*****************************************************/
// This line is part of the initial check sequence and is not
// needed for normal operation:
desiredCounter = 0;
trigger = 0;
stepPulses = 0;
currentCounter = 0;
set_motor_output( 0, 0 );
/****************** ENABLE INTERRUPTS *****************/
INTCONbits.GIEH = 1; // Enable global interrupts
INTCONbits.GIEL = 1; // Enable peripheral innterupts
/******************************************************/
while (1)
{
INTCONbits.GIEH = 0;
tempTrigger = trigger;
INTCONbits.GIEH = 1;
if ( tempTrigger )
{
INTCONbits.GIEH = 0;
trigger = 0;
tempCurrentCounter = currentCounter;
tempStepPulses = stepPulses;
stepPulses = 0;
// Check if step pin has been held HIGH. If it has been held HIGH
// long enough to indicate a HOME command, set home flag, otherwise
// just increment stepHighCounter. stepHighCounter gets reset in
// in interrupt routine that reads step and direction lines.
if (stepHighCounter) {
if (stepHighCounter > stepHomeThreshold) {
homeFlag = 1;
stepHighCounter = 0;
homeDir = PORTBbits.RB1; //set home direction to direction input pin
}
else {
if (PORTBbits.RB0) {
stepHighCounter++;
}
}
}
//stepTimerFoo++;
//tempStepTimerFoo = stepTimerFoo;
INTCONbits.GIEH = 1;
// if tempStepTimerFoo > something
// then set homeFlag, set homeDir
// if homeflag
// if homeswitch
// turn off homing
// else
// set motor go to home in homeDir direction
// end
//else
// do normal motor control
//end
/*
INTCONbits.GIEH = 0;
trigger = 0;
INTCONbits.GIEH = 1;
//if (PORTAbits.RA1)
// PORTAbits.RA1 = 0;
//else
// PORTAbits.RA1 = 1;
INTCONbits.GIEH = 0;
tempCurrentCounter = currentCounter;
tempStepPulses = stepPulses;
stepPulses = 0;
INTCONbits.GIEH = 1; */
// velocity = (tempCurrentCounter - prevCounter)*counterToSpeedScalar;
// prevCounter = tempCurrentCounter;
// Uncomment this for normal operation
desiredCounter = desiredCounter + (tempStepPulses*numCountsPerStep);
if (homeFlag) // If we are in homing mode
{
if (PORTAbits.RA0) // if homeswitch is ON
{
// We're home!
set_motor_output(0,0);
homeFlag = 0;
desiredCounter = 0;
// turn off interrupts to clear sensitive variables
INTCONbits.GIEH = 0;
currentCounter = 0;
INTCONbits.GIEH = 1;
}
else // if homeswitch is OFF - we are still going home
{
if (homeDir) // going up
{
set_motor_output(abs(upTorqueLimit),1);
}
else // going down
{
set_motor_output(abs(downTorqueLimit), 0);
}
}
}
else
{
error = desiredCounter - tempCurrentCounter;
output = Kp * error;
////////////// Set motor output //////////////////////////////
//
// The variable "output" controls voltage to motor. Examples:
//
// this makes the motor go up:
// set_motor_output(30,1);
//
// this makes the motor go down:
// set_motor_output(30,0);
//
if (output <= 0)
{
if (output < downTorqueLimit)
{
output = downTorqueLimit;
}
set_motor_output(abs(output),0);
}
else
{
if (output > upTorqueLimit)
{
output = upTorqueLimit;
}
set_motor_output(abs(output),1);
}
}
}
}
}
