祝国内电动汽车发力发展，分享电动汽车代码给大家
10/31/03 2.00 Released Motor runs fine, still some loose ends
*
*12/19/03 2.01 Cleaned up structure, created UserParms.h for all
* user defines.
*
*02/12/04 3.00 -Removed unnecessary files from project.
-Changed iRPM to int to correct floating point calc
* problems.
* -CalcVel() and velocity control loop only execute
* after number of loop periods specified by
* iIrpPerCalc.
* -Added iDispLoopCount variable to schedule
* execution of display and button routines
* -trig.s file changed to use program space for
* storage of sine data.
* -Added DiagnosticsOutput() function that uses
* output compare channels to output control variable
* information.
* -Added TORQUE_MODE definition to bypass velocity
* control loop.
* -Turned off SATDW bit in curmodel.s file. The
* automatic saturation feature prevents slip
* angle calculation from wrapping properly.
*
* 11/03/05 4.00 -Added support to software for MPLAB tuning
* interface. This includes the snapshot code.
* Also modified code so that user parameters
* only get initialized at user startup.
* 15/06/07 5.00 -Added PID tuning support from LABVIEW application
* over CAN bus. Added support for regenerative braking.
* PID parameters are now stored in EEPROM memory.
************************************************************************
* Code Description
*
* This file demonstrates Vector Control of a 3 phase ACIM using the
* dsPIC30F. SVM is used as the modulation strategy.
**********************************************************************/
/************** GLOBAL DEFINITIONS ***********/
#define INITIALIZE
#include
#include
#include "Motor.h"
#include "Parms.h"
#include "Encoder.h"
#include "SVGen.h"
#include "ReadADC.h"
#include "MeasCurr.h"
#include "CurModel.h"
#include "FdWeak.h"
#include "Control.h"
#include "PI.h"
#include "Park.h"
#include "OpenLoop.h"
#include "LCD.h"
#include "bin2dec.h"
#include "UserParms.h"
//inlcudes for the CAN1 module
#include
#include "delay.h"
#include "can.h"
#include "uart.h"
#include "stdlib.h"
#include "CAN1IsTXReady.c"
#include "CAN1SendMessage.c"
#include "CAN1Initialize.c"
#include "CAN1SetOperationMode.c"
/*****************Config bit settings****************/
//_FOSC(CSW_FSCM_OFF & XT_PLL8);
//_FWDT(WDT_OFF);
//_FBORPOR(PBOR_ON & BORV_20 & PWRT_64 & MCLR_EN & PWMxH_ACT_LO & PWMxL_ACT_LO);
///************** END OF GLOBAL DEFINITIONS ***********/
#define TORQUE_MODE //Motor primarily works on torque mode
//#define DIAGNOSTICS
//#define SNAPSHOT
#ifdef SNAPSHOT
#define SNAPSIZE 900
#define SNAP_DELAY SNAPSIZE/4
#define SNAP1 ParkParm.qIq
#define SNAP2 ParkParm.qId
#define SNAP3 EncoderParm.qVelMech
int SnapBuf1[SNAPSIZE];
int SnapBuf2[SNAPSIZE];
int SnapBuf3[SNAPSIZE];
int SnapCount;
#endif
unsigned short uWork;
short iCntsPerRev;
short iDeltaPos;
short accelvalue = 0;
short accelvaluelast = 0;
short brakevalue = 0;
short brakevaluelast = 0;
short fluxLast = 0;
union {
struct
{
unsigned DoLoop:1;
unsigned BeginStop:1;
unsigned DoSnap:1;
unsigned SnapDone:1;
unsigned OpenLoop:1;
unsigned RunMotor:1;
unsigned RunningUpload:1;
unsigned Btn1Pressed:1;
unsigned Btn2Pressed:1;
unsigned Btn3Pressed:1;
unsigned Btn4Pressed:1;
unsigned UploadToMonitor:1;
unsigned ChangeMode:1;
unsigned ChangeSpeed:1;
unsigned :2;
}bit;
WORD Word;
} uGF; // general flags
union {
long Long;
WORD Word[2];
short Short[2];
BYTE Byte[4];
} DataUnion;
tPIParm PIParmQ;
tPIParm PIParmQref;
tPIParm PIParmD;
tReadADCParm ReadADCParm;
tReadADCParm ReadADC3Parm;
char ResetAngle;//RG
char brake,Flaggy, RecFlag = 0;
int temp;
int iRPM,iRPM_old,RPMbuf,RPMave[100], count = 0, i = 0, Torqbuf = 0, Fluxbuf = 0;
short qPID[9] = {1,2,3,4,5,6,7,8,9};
WORD iMaxLoopCnt;
WORD iLoopCnt;
WORD iDispLoopCnt;
/*Declare constants/coefficients/calibration data to be stored in DataEEPROM*/
int _EEDATA(32) fooArrayInDataEE[] = {0,1,2,3,4,5,6,7,8,9,0xA,0xB,0xC,0xD,0xE,0xF};
/*Declare variables to be stored in RAM*/
int fooArray1inRAM[] = {0xAAAA, 0xBBBB, 0xCCCC, 0xDDDD, 0xEEEE, 0xFFFF, 0xABCD, 0xBCDE,
0xCDEF, 0xDEFA, 0x0000, 0x1111, 0x2222, 0x3333, 0x4444, 0x5555};
short fooArray2inRAM[9];
/******************CAN Variables*********************/
unsigned char send_buffer1[] = {0x00,0x00,0x0}, send_buffer2[] = {0x00,0x00,0x00}, send_buffer3[] = {0x00,0x00,0x00}, receive_buffer[8] = {0,0,0,0,0,0,0,0}, ack_buf[1] = {0x41}, nack_buf[1] = {0x4E};
unsigned char buffer_ready, rx_ready, test;
float rec = 0, dataf[9] = {0,0,0,0,0,0,0,0,0};
short data[9] = {0,0,0,0,0,0,0,0,0};
unsigned char chk1 = 0, chk2 = 0;
int timeout1 = 0, timeout2 = 0, time_flag = 0;
/******************************************************/
void __attribute__((__interrupt__)) _ADCInterrupt(void);
void SetupBoard( void );
bool SetupPeripherals(void);
void InitUserParms(void);
void DoControl( void );
void Dis_RPM( BYTE bChrPosC, BYTE bChrPosR );
void DiagnosticsOutput(void);
void init(void); //can init
void GetRPM(void);
void GetTorq(void);
void GetFlux(void);
void GetqPIDParms(void);
/************* START OF MAIN FUNCTION ***************/
int main ( void )
{
ResetAngle = 0; //flux angle control reset
int temp = 0;
for( i = 0; i == 100; i++) //clear average RPM buffer
{
RPMave[i] = 0;
}
i = 0;
init(); //CAN bus initialization
/* Initialize Serial RS 232 (Port B on dev board, 115.2kBaud at 12.29MIPS) */ //RG: this block
U1BRG = 10; //Set baud rate. At 12.29 MIPS, should result in 115.2kbps - 4.8 %. Seems to work regardless.
U1MODEbits.PDSEL = 0b00; //8 bits, no parity
U1MODEbits.STSEL = 0; //1 stop bit
U1MODEbits.UARTEN = 1; //enable UART, also enables receive
U1STAbits.UTXEN = 1; //enable transmit
U1TXREG = (unsigned char)fooArray2inRAM[7]; //show startup successful on UART
Wait(0xFFFF);
SetupPorts();
while(1)
{
GetqPIDParms();//Receive PID parameters over CAN bus and store into array
if((chk1 == 9)&&(chk2 == 9))//if all parameters have been received
{
chk1 = 0; //clear reception check variables
chk2 = 0;
Wait(0xFFFF);
//set normal mode
CAN1SetOperationMode(CAN_CAPTURE_EN & CAN_IDLE_CON & CAN_MASTERCLOCK_1 & CAN_REQ_OPERMODE_NOR);
Wait(0x000F);
//send ack to LABVIEW application
CAN1SendMessage(1, 1, ack_buf, 0x01,0x00);
while(!CAN1IsTXReady(0));
/*Erase 16 words (1 row in dsPIC30F DataEEPROM) in Data EEPROM from array named "fooArrayinDataEE" */
temp = EraseEE(__builtin_tblpage(&fooArrayInDataEE[0]), __builtin_tbloffset(&fooArrayInDataEE[0]), 16);
/*Write 16 words (1 row in dsPIC30F DataEEPROM) to Data EEPROM from array named "fooArray1inRAM" */
/*to array named "fooArrayinDataEE" */
temp = WriteEE(&qPID[0],__builtin_tblpage(&fooArrayInDataEE[0]),__builtin_tbloffset(&fooArrayInDataEE[0]), 16);
//break;
RecFlag = 1;
}
if((chk1 != chk2)||(time_flag == 1))//if all parameters have not been received
{
//set normal mode
CAN1SetOperationMode(CAN_CAPTURE_EN & CAN_IDLE_CON & CAN_MASTERCLOCK_1 & CAN_REQ_OPERMODE_NOR);
//send nack
CAN1SendMessage(1, 1, nack_buf, 0x01,0x00);
while(!CAN1IsTXReady(0));
chk1 = 0; //clear reception check variables
chk2 = 0;
time_flag = 0; //clear timeout flag
//set listen mode
CAN1SetOperationMode(CAN_CAPTURE_EN & CAN_IDLE_CON & CAN_MASTERCLOCK_1 & CAN_REQ_OPERMODE_LISTENALL);
//break;
RecFlag = 0;
}
if(pinButton1)//if button pressed then break and start motor normal operation mode
{
//set CAN bus to normal operation
CAN1SetOperationMode(CAN_CAPTURE_EN & CAN_IDLE_CON & CAN_MASTERCLOCK_1 & CAN_REQ_OPERMODE_NOR);
Wait(0x000F);
break;
}
}
//Initialized user parameters
InitUserParms();
while(1)
{
uGF.Word = 0; // clear flags
// init Mode
uGF.bit.OpenLoop = 0; // start in closed loop
// init LEDs
pinLED1 = 0;
pinLED2 = !uGF.bit.OpenLoop;
pinLED3 = 0;
pinLED4 = 0;
// init board
SetupBoard();
// init user specified parms and stop on error
if( SetupPeripherals() )
{
// Error
uGF.bit.RunMotor=0;
return;
}
// zero out i sums
PIParmD.qdSum = 0;
PIParmQ.qdSum = 0;
PIParmQref.qdSum = 0;
iMaxLoopCnt = 0;
// Enable ADC interrupt and begin main loop timing
IFS0bits.ADIF = 0;
IEC0bits.ADIE = 1;
if(!uGF.bit.RunMotor)
{
// Initialize current offset compensation
while(!pinButton1) //wait here until button 1 is pressed
{
ClrWdt();
// Start offset accumulation //and accumulate current offset while waiting
MeasCompCurr();
}
while(pinButton1); //when button 1 is released
uGF.bit.RunMotor = 1; //then start motor
}
// Run the motor
uGF.bit.ChangeMode = 1;
// Enable the driver IC on the motor control PCB
pinPWMOutputEnable_ = 0;
//Run Motor loop
while(1)
{
ClrWdt();
// The code that updates the LCD display and polls the buttons
// executes every 50 msec.
if(iDispLoopCnt >= dDispLoopCnt)
{
Dis_RPM(5,0);
// Button 1 starts or stops the motor
if(pinButton1)
{
if( !uGF.bit.Btn1Pressed )
uGF.bit.Btn1Pressed = 1;
}
else
{
if( uGF.bit.Btn1Pressed )
{
// Button just released
uGF.bit.Btn1Pressed = 0;
// begin stop sequence
uGF.bit.RunMotor = 0;
pinPWMOutputEnable_ = 1;
break;
}
}
} // end of display and button polling code
//********************************************************************************************************
//CAN STUFF FOLLOWS
//Calculate RPM and send it to LABVIEW application
GetRPM();
while(!CAN1IsTXReady(0));
IEC0bits.ADIE = 0;
CAN1SendMessage(1, 1, send_buffer1, 0x03,0x00);
IEC0bits.ADIE = 1;
while(!CAN1IsTXReady(0));
// Wait(0x00FF);
//Calculate torque and send it to LABVIEW application
GetTorq();
while(!CAN1IsTXReady(1));
IEC0bits.ADIE = 0;
CAN1SendMessage(1, 1, send_buffer2, 0x03,0x01);
IEC0bits.ADIE = 1;
while(!CAN1IsTXReady(1));
// Wait(0x00FF);
//Calculate flux and send it to LABVIEW application
GetFlux();
while(!CAN1IsTXReady(2));
IEC0bits.ADIE = 0;
CAN1SendMessage(1, 1, send_buffer3, 0x03,0x02);
IEC0bits.ADIE = 1;
while(!CAN1IsTXReady(2));
//Wait(0xFFF);
//*************************************************************************************************************
} // End of Run Motor loop
} // End of Main loop
// should never get here
while(1){}
}
//---------------------------------------------------------------------
// Executes one PI itteration for each of the three loops Id,Iq,Speed
void DoControl( void )
{
short i;
// Assume ADC channel 0 has raw A/D value in signed fractional form from
// speed pot (AN7).
ReadADC0( &ReadADCParm );
if(((unsigned int)ReadADCParm.qADValue < 100 )) //small deadband at lowest range to pot
{
pinPWMOutputEnable_ = 1; //disable PWM outputs
ReadADCParm.qADValue = 0;
brake = 0;
}
else
{
pinPWMOutputEnable_ = 0; //enable PWM outputs
//use deadbands for gas and brake
// if((unsigned int)ReadADCParm1.qADValue < 100 )
// {
// ReadADCParm1.qADValue = 0;
// brake = 0;
// }
// if((unsigned int)ReadADCParm.qADValue < 100 )
// {
// ReadADCParm.qADValue = 0;
// }
}
//Regenerative Braking support not used but can be added
//accelvalue = ReadADCParm.qADValue.
//accelvalue = ((0x7FFF & (0x7FFF - (unsigned short)ReadADCParm.qADValue))-9251)*2; //scaling
//accelvalue += accelvalue/5; //scaling
//the addition and multiplication was resulting in a roll overso that sometimes when the pedal was fully pressed it
//equaled -32000 the following code says that if the value goes negative and the last value measured was over 30000 then
//the negative value is a result of rollover so set the value to maximum
// if(accelvalue < 0 && accelvaluelast > 30000)
// accelvalue = 32767;
//due to scaling there was a negative value when the pedal is released. since negative values are not wanted set value to
//zero
// if(accelvalue < 0)
// accelvalue = 0;
// accelvaluelast = accelvalue; //last value measured
// ReadADCParm.qADValue = accelvalue; //put the scaled value into the variable used for speed control
//Set speed/torque reference
CtrlParm.qVelRef = ReadADCParm.qADValue >> 1;
if( uGF.bit.ChangeMode )
{
// just changed from openloop
uGF.bit.ChangeMode = 0;
// synchronize angles and prep qdImag
CurModelParm.qAngFlux = OpenLoopParm.qAngFlux;
CurModelParm.qdImag = ParkParm.qId;
}
// Current model calculates angle
CurModelParm.qVelMech = EncoderParm.qVelMech;
CurModel();
ParkParm.qAngle = CurModelParm.qAngFlux;
// Calculate qVdRef from field weakening
FdWeakening();
// Check to see if new velocity information is available by comparing
// the number of interrupts per velocity calculation against the
// number of velocity count samples taken. If new velocity info
// is available, calculate the new velocity value and execute
// the speed control loop.
if(EncoderParm.iVelCntDwn == EncoderParm.iIrpPerCalc)
{
// Calculate velocity from acumulated encoder counts
CalcVel();
// Execute the velocity control loop
PIParmQref.qInMeas = EncoderParm.qVelMech;
PIParmQref.qInRef = CtrlParm.qVelRef;
CalcPI(&PIParmQref);
CtrlParm.qVqRef = PIParmQref.qOut;
}
// If the application is running in torque mode, the velocity
// control loop is bypassed. The velocity reference value, read
// from the potentiometer, is used directly as the torque
// reference, VqRef.
#ifdef TORQUE_MODE
CtrlParm.qVqRef = CtrlParm.qVelRef;
#endif
// PI control for Q
PIParmQ.qInMeas = ParkParm.qIq;
PIParmQ.qInRef = CtrlParm.qVqRef;
CalcPI(&PIParmQ);
ParkParm.qVq = PIParmQ.qOut;
// PI control for D
PIParmD.qInMeas = ParkParm.qId;
PIParmD.qInRef = CtrlParm.qVdRef;
CalcPI(&PIParmD);
ParkParm.qVd = PIParmD.qOut;
}
//---------------------------------------------------------------------
// The ADC ISR does speed calculation and executes the vector update loop.
// The ADC sample and conversion is triggered by the PWM period.
// The speed calculation assumes a fixed time interval between calculations.
//---------------------------------------------------------------------
void __attribute__((__interrupt__)) _ADCInterrupt(void)
{
IFS0bits.ADIF = 0;
// Increment count variable that controls execution
// of display and button functions.
iDispLoopCnt++;
// acumulate encoder counts since last interrupt
CalcVelIrp();
if( uGF.bit.RunMotor )
{
// Set LED1 for diagnostics
pinLED1 = 1;
// use TMR1 to measure interrupt time for diagnostics
TMR1 = 0;
iLoopCnt = TMR1;
// Calculate qIa,qIb
MeasCompCurr();
// Calculate qId,qIq from qSin,qCos,qIa,qIb
ClarkePark();
// Calculate control values
DoControl();
// Calculate qSin,qCos from qAngle
SinCos();
// Calculate qValpha, qVbeta from qSin,qCos,qVd,qVq
InvPark();
// Calculate Vr1,Vr2,Vr3 from qValpha, qVbeta
CalcRefVec();
// Calculate and set PWM duty cycles from Vr1,Vr2,Vr3
CalcSVGen();
// Measure loop time
iLoopCnt = TMR1 - iLoopCnt;
if( iLoopCnt > iMaxLoopCnt )
iMaxLoopCnt = iLoopCnt;
// Clear LED1 for diagnostics
pinLED1 = 0;
}
}
//---------------------------------------------------------------------
// SetupBoard
//
// Initialze board
//---------------------------------------------------------------------
void SetupBoard( void )
{
BYTE b;
// Disable ADC interrupt
IEC0bits.ADIE = 0;
// Reset any active faults on the motor control power module.
//pinFaultReset = 1;
//for(b=0;b<10;b++)
Nop();
//pinFaultReset = 0;
// Ensure PFC switch is off.
pinPFCFire = 0;
// Ensure brake switch is off.
pinBrakeFire = 0;
}
//---------------------------------------------------------------------
// Dis_RPM
//
// Display RPM
//---------------------------------------------------------------------
void Dis_RPM( BYTE bChrPosC, BYTE bChrPosR )
{
iRPM_old = iRPM;
iRPM = EncoderParm.iDeltaCnt*60/(MotorParm.fLoopPeriod*MotorParm.iIrpPerCalc*EncoderParm.iCntsPerRev);
iRPM = (iRPM + iRPM_old) >> 1;
}
//---------------------------------------------------------------------
void InitUserParms(void)
{
int dummy =0, index = 0xF000, num = 0;
// Turn saturation on to insure that overflows will be handled smoothly.
CORCONbits.SATA = 0;
// Setup required parameters
// Pick scaling values to be 8 times nominal for speed and current
// Use 8 times nominal mechanical speed of motor (in RPM) for scaling
MotorParm.iScaleMechRPM = diNomRPM*8;
// Number of pole pairs
MotorParm.iPoles = diPoles ;
// Encoder counts per revolution as detected by the
// dsPIC quadrature configuration.
MotorParm.iCntsPerRev = diCntsPerRev;
// Rotor time constant in sec
MotorParm.fRotorTmConst = dfRotorTmConst;
// Basic loop period (in sec). (PWM interrupt period)
MotorParm.fLoopPeriod = dLoopInTcy * dTcy; //Loop period in cycles * sec/cycle
// Encoder velocity interrupt period (in sec).
MotorParm.fVelIrpPeriod = MotorParm.fLoopPeriod;
// Number of vel interrupts per velocity calculation.
MotorParm.iIrpPerCalc = diIrpPerCalc; // In loops
// Scale mechanical speed of motor (in rev/sec)
MotorParm.fScaleMechRPS = MotorParm.iScaleMechRPM/60.0;
// Scaled flux speed of motor (in rev/sec)
// All dimensionless flux velocities scaled by this value.
MotorParm.fScaleFluxRPS = MotorParm.iPoles*MotorParm.fScaleMechRPS;
// Minimum period of one revolution of flux vector (in sec)
MotorParm.fScaleFluxPeriod = 1.0/MotorParm.fScaleFluxRPS;
// Fraction of revolution per LoopTime at maximum flux velocity
MotorParm.fScaleFracRevPerLoop = MotorParm.fLoopPeriod * MotorParm.fScaleFluxRPS;
// Scaled flux speed of motor (in radians/sec)
// All dimensionless velocities in radians/sec scaled by this value.
MotorParm.fScaleFluxSpeed = 6.283 * MotorParm.fScaleFluxRPS;
// Encoder count rate at iScaleMechRPM
MotorParm.lScaleCntRate = MotorParm.iCntsPerRev * (MotorParm.iScaleMechRPM/60.0);
// ============= Open Loop ======================
OpenLoopParm.qKdelta = 32768.0 * 2 * MotorParm.iPoles * MotorParm.fLoopPeriod * MotorParm.fScaleMechRPS;
OpenLoopParm.qVelMech = dqOL_VelMech;
CtrlParm.qVelRef = OpenLoopParm.qVelMech;
InitOpenLoop();
// ============= Field Weakening ===============
// Field Weakening constant for constant torque range
FdWeakParm.qK1 = dqK1; // Flux reference value
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++=
//Load Coefficients from EEPROM
/*Read array named "fooArrayinDataEE" from DataEEPROM and place the result into*/
/*array in RAM named, "fooArray2inRAM" */
temp = ReadEE(__builtin_tblpage(&fooArrayInDataEE[0]),__builtin_tbloffset(&fooArrayInDataEE[0]),&fooArray2inRAM[0], 16);
//Read EEPROM array and write to UART for trouble shooting
data[0] = fooArray2inRAM[0];
dataf[0] = (float)data[0]/10000;
U1TXREG = (unsigned char)data[0];
Wait(0xFFFF);
data[1] = fooArray2inRAM[1];
dataf[1] = (float)data[1]/10000;
U1TXREG = (unsigned char)data[1];
Wait(0xFFFF);
data[2] = fooArray2inRAM[2];
dataf[2] = (float)data[2]/10000;
U1TXREG = (unsigned char)data[2];
Wait(0xFFFF);
data[3] = fooArray2inRAM[3];
dataf[3] = (float)data[3]/10000;
U1TXREG = (unsigned char)data[3];
Wait(0xFFFF);
data[4] = fooArray2inRAM[4];
dataf[4] = (float)data[4]/10000;
U1TXREG = (unsigned char)data[4];
Wait(0xFFFF);
data[5] = fooArray2inRAM[5];
dataf[5] = (float)data[5]/10000;
U1TXREG = (unsigned char)data[5];
Wait(0xFFFF);
data[6] = fooArray2inRAM[6];
dataf[6] = (float)data[6]/10000;
U1TXREG = (unsigned char)data[6];
Wait(0xFFFF);
data[7] = fooArray2inRAM[7];
dataf[7] = (float)data[7]/10000;
U1TXREG = (unsigned char)data[7];
Wait(0xFFFF);
data[8] = fooArray2inRAM[8];
dataf[8] = (float)data[8]/10000;
U1TXREG = (unsigned char)data[8];
Wait(0xFFFF);
// if(RecFlag == 1) //If PID params have been received over CAN bus then load them into RAM
// {
// ============= PI D Term ===============
PIParmD.qKp = data[0];
PIParmD.qKi = data[1];
PIParmD.qKc = data[2];
PIParmD.qOutMax = dDqOutMax;
PIParmD.qOutMin = -PIParmD.qOutMax;
InitPI(&PIParmD);
// ============= PI Q Term ===============
PIParmQ.qKp = data[3];
PIParmQ.qKi = data[4];
PIParmQ.qKc = data[5];
PIParmQ.qOutMax = dQqOutMax;
PIParmQ.qOutMin = -PIParmQ.qOutMax;
InitPI(&PIParmQ);
// ============= PI Qref Term ===============
PIParmQref.qKp = data[6];
PIParmQref.qKi = data[7];
PIParmQref.qKc = data[8];
PIParmQref.qOutMax = dQrefqOutMax;
PIParmQref.qOutMin = -PIParmQref.qOutMax;
InitPI(&PIParmQref);
// }
// else //load PID params from RAM for trouble shooting
// {
// // ============= PI D Term ===============
// PIParmD.qKp = dDqKp;
// PIParmD.qKi = dDqKi;
// PIParmD.qKc = dDqKc;
// PIParmD.qOutMax = dDqOutMax;
// PIParmD.qOutMin = -PIParmD.qOutMax;
//
// InitPI(&PIParmD);
//
// // ============= PI Q Term ===============
// PIParmQ.qKp = dQqKp;
// PIParmQ.qKi = dQqKi;
// PIParmQ.qKc = dQqKc;
// PIParmQ.qOutMax = dQqOutMax;
// PIParmQ.qOutMin = -PIParmQ.qOutMax;
//
// InitPI(&PIParmQ);
//
// // ============= PI Qref Term ===============
// PIParmQref.qKp = dQrefqKp;
// PIParmQref.qKi = dQrefqKi;
// PIParmQref.qKc = dQrefqKc;
// PIParmQref.qOutMax = dQrefqOutMax;
// PIParmQref.qOutMin = -PIParmQref.qOutMax;
//
// InitPI(&PIParmQref);
//
// }
// ============= ADC - Measure Current & Pot ======================
// Scaling constants: Determined by calibration or hardware design.
ReadADCParm.qK = dqK;
ReadADC3Parm.qK = dqK;
MeasCurrParm.qKa = dqKa;
MeasCurrParm.qKb = dqKb;
// Inital offsets
InitMeasCompCurr( 450, 730 );
}
//---------------------------------------------------------------------
bool SetupPeripherals(void)
{
// ============= Encoder ===============
if( InitEncoderScaling() )
// Error
return True;
// ============= Current Model ===============
if(InitCurModelScaling())
// Error
return True;
// ============= SVGen ===============
// Set PWM period to Loop Time
SVGenParm.iPWMPeriod = dLoopInTcy;
// ============= TIMER #1 ======================
PR1 = 0xFFFF;
T1CONbits.TON = 1;
T1CONbits.TCKPS = 1; // prescale of 8 => 1.08504 uS tick
// ============= Motor PWM ======================
PDC1 = 0;
PDC2 = 0;
PDC3 = 0;
PDC4 = 0;
// Center aligned PWM.
// Note: The PWM period is set to dLoopInTcy/2 but since it counts up and
// and then down => the interrupt flag is set to 1 at zero => actual
// interrupt period is dLoopInTcy
PTPER = dLoopInTcy/2; // Setup PWM period to Loop Time defined in parms.h
PWMCON1 = 0x0077; // Enable PWM 1,2,3 pairs for complementary mode
DTCON1 = dDeadTime; // Dead time
DTCON2 = 0;
FLTACON = 0; // PWM fault pins not used
FLTBCON = 0;
PTCON = 0x8002; // Enable PWM for center aligned operation
// SEVTCMP: Special Event Compare Count Register
// Phase of ADC capture set relative to PWM cycle: 0 offset and counting up
SEVTCMP = 2; // Cannot be 0 -> turns off trigger (Missing from doc)
SEVTCMPbits.SEVTDIR = 0;
// ============= Encoder ===============
MAXCNT = MotorParm.iCntsPerRev;
POSCNT = 0;
QEICON = 0;
QEICONbits.QEIM = 7; // x4 reset by MAXCNT pulse
QEICONbits.POSRES = 0; // Don't allow Index pulse to reset counter
QEICONbits.SWPAB = 1; // direction
DFLTCON = 0; // Digital filter set to off
// ============= ADC - Measure Current & Pot ======================
// ADC setup for simultanous sampling on
// CH0=AN7, CH1=AN0, CH2=AN1, CH3=AN2.
// Sampling triggered by PWM and stored in signed fractional form.
ADCON1 = 0;
// Signed fractional (DOUT = sddd dddd dd00 0000)
ADCON1bits.FORM = 3;
// Motor Control PWM interval ends sampling and starts conversion
ADCON1bits.SSRC = 3;
// Simultaneous Sample Select bit (only applicable when CHPS = 01 or 1x)
// Samples CH0, CH1, CH2, CH3 simultaneously (when CHPS = 1x)
// Samples CH0 and CH1 simultaneously (when CHPS = 01)
ADCON1bits.SIMSAM = 1;
// Sampling begins immediately after last conversion completes.
// SAMP bit is auto set.
ADCON1bits.ASAM = 1;
ADCON2 = 0;
// Samples CH0, CH1, CH2, CH3 simultaneously (when CHPS = 1x)
ADCON2bits.CHPS = 2;
ADCON3 = 0;
// A/D Conversion Clock Select bits = 8 * Tcy
ADCON3bits.ADCS = 15;
/* ADCHS: ADC Input Channel Select Register */
ADCHS = 0;
// CH0 is AN7
ADCHSbits.CH0SA = 7;
// CH1 positive input is AN0, CH2 positive input is AN1, CH3 positive input is AN2
ADCHSbits.CH123SA = 0;
/* ADPCFG: ADC Port Configuration Register */
// Set all ports digital
ADPCFG = 0xFFFF;
ADPCFGbits.PCFG0 = 0; // AN0 analog
ADPCFGbits.PCFG1 = 0; // AN1 analog
ADPCFGbits.PCFG2 = 0; // AN2 analog
ADPCFGbits.PCFG7 = 0; // AN7 analog
/* ADCSSL: ADC Input Scan Select Register */
ADCSSL = 0;
// Turn on A/D module
ADCON1bits.ADON = 1;
return False;
}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
Function: init
Description: This function will initialize the CAN bus module for the purpose
of communicating with the LABVIEW interface. The default mode will be
LISTENALL after this code is executed. The default bit rate is 125 kbps with
an oscillator frequency of 20MHz.
Input arguments: void
Output arguments: void
*/
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void init(void)
{
//set up registers fCAN = fOSC
//normal operation mode
//no abort
CAN1SetOperationMode(CAN_CAPTURE_EN & CAN_IDLE_CON & CAN_MASTERCLOCK_1 & CAN_REQ_OPERMODE_CONFIG);
//set baud rate pre scale(BRP) to 6, phase segments to 8 TQ each and Propagation time to 8 TQ.
CAN1Initialize(CAN_SYNC_JUMP_WIDTH1 & CAN_BAUD_PRE_SCALE(19), CAN_PHASE_SEG2_TQ(6) & CAN_PHASE_SEG1_TQ(7)& CAN_PROPAGATIONTIME_SEG_TQ(2));
CAN1SetOperationMode(CAN_CAPTURE_EN & CAN_IDLE_CON & CAN_MASTERCLOCK_1 & CAN_REQ_OPERMODE_NOR);
// CAN1SetOperationMode(CAN_CAPTURE_EN & CAN_IDLE_CON & CAN_MASTERCLOCK_1 & CAN_REQ_OPERMODE_LOOPBK);
// //set up registers
// //transmit registers
C1TX0CON = 0x0003;//buffer zero, highest priority
C1TX1CON = 0x0002;//buffer one, high intermediate priority
C1TX0CON = 0x0001;//buffer two, low intermediate priority
//
// //receive register
C1RX0CON = 0x0004;//allow buffer rollover, allow jump table offset between 0 and 1
//SID
C1RX0SID = 0X0000;
C1RX1SID = 0X0000;
C1RX0EID = 0X0000;
C1RX1EID = 0X0000;
C1CTRLbits.CANCAP = 1;
// //message acceptance filters //buffer 0//the masks are all set to zero so all filter bits are not considered (C1RXMnSID)
C1RXF0SID = 0x0000;//EXIDE = 1//enable filter for standard identifiers, the rest of this register is used for id bits
// //bits 12-2 ===== SID <10:0>
C1RXF0EIDH = 0X0000;
C1RXF0EIDL = 0X0000;
C1RXM0SID = 0x0000;//MIDE set to one to match only standard messages as determined by EXIDE
C1RXM0EIDH = 0x0000;
C1RXM0EIDL = 0x0000;
//
// //buffer 1
C1RXF1SID = 0x0000;//EXIDE = 1//enable filter for standard identifiers, the rest of this register is used for id bits
// //bits 12-2 ===== SID <10:0>
C1RXF1EIDH = 0X0000;
C1RXF1EIDL = 0X0000;
C1RXM1SID = 0x0000;//MIDE set to one to match only standard messages as determined by EXIDE
C1RXM1EIDH = 0x0000;
C1RXM1EIDL = 0x0000;
/* Load mask and filter registers */
CAN1SetFilter(0, CAN_FILTER_SID(0) & CAN_RX_EID_DIS, CAN_FILTER_EID(0));
CAN1SetMask(0, CAN_MASK_SID(0) & CAN_IGNORE_FILTER_TYPE, CAN_MASK_EID(0));
/* Set transmitter and receiver modes */
CAN1SetTXMode(0,CAN_TX_REQ & CAN_TX_PRIORITY_HIGH );
CAN1SetTXMode(1,CAN_TX_REQ & CAN_TX_PRIORITY_HIGH );
CAN1SetTXMode(2,CAN_TX_REQ & CAN_TX_PRIORITY_HIGH );
CAN1SetRXMode(0,CAN_RXFUL_CLEAR &CAN_BUF0_DBLBUFFER_EN);
CAN1SetOperationMode(CAN_CAPTURE_EN & CAN_IDLE_CON & CAN_MASTERCLOCK_1 & CAN_REQ_OPERMODE_LISTENALL);
}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
Function: GetRPM
Description: This function will add up 100 RPM readings and vaverage them.
The value of RPM is global and is not modified by this function.
The send_buffer1 will be modified once the average has been calculated and
will have and identifying byte appended to it.
Input arguments: void
Output arguments: void
*/
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void GetRPM(void)
{
//if(count < 100)
// {
// RPMbuf = 0;
// RPMave[count] = iRPM;
// count++;
// }
// if(count >= 100)
// {
// for( i = 0; i == 100; i++)
// {
// RPMbuf = RPMbuf + RPMave[i];
// }
// RPMbuf = RPMbuf/100;
RPMbuf = iRPM;
//RPMbuf = 700;
if(RPMbuf > 2000)
{
RPMbuf = 2000;
}
if(RPMbuf < 0)
{
RPMbuf = 0;
}
send_buffer1[0] = 0x01;
send_buffer1[1] = (unsigned char)RPMbuf;
RPMbuf = RPMbuf >> 8;
send_buffer1[2] = (unsigned char)RPMbuf;
if(send_buffer1[2] == 0x00)
{
send_buffer1[2] = 0xFF;//bit stuffing for LABVIEW application
}
if((send_buffer1[2] == 0x00)&&(send_buffer1[1] == 0x00))
{
send_buffer1[1] = 0xFE;//bit stuffing for LABVIEW application
send_buffer1[2] = 0xFE;//bit stuffing for LABVIEW application
}
count = 0;
// }
}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
Function: GetTorq
Description: This function will get the current torque. The value of torque is
global and is not modified by this function. The send_buffer2 will be modified
once the average has been calculated and will have and identifying byte appended
to it.
Input arguments: void
Output arguments: void
*/
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void GetTorq(void)
{
unsigned int maskpos = 0x8000, maskneg = 0x7FFF, maskbit = 0x8000;
unsigned int value = 0, masknum = 0;
value = ParkParm.qIq; //get current torque value
masknum = value & maskbit;
if(masknum == 0)
{
value = value | maskpos;
}
else
{
value = value & maskneg;
}
Torqbuf = value;
//load CAN bus buffer
send_buffer2[0] = 0x02;
send_buffer2[1] = (unsigned char)Torqbuf;
Torqbuf = Torqbuf >> 8;
send_buffer2[2] = (unsigned char)Torqbuf;
if(send_buffer1[2] == 0x00)
{
send_buffer1[2] = 0xFF; //bit stuffing for LABVIEW application
}
}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
Function: GetFlux
Description: This function will get the current flux. The value of flux is
global and is not modified by this function. The send_buffer3 will be modified
once the average has been calculated and will have and identifying byte appended
to it.
Input arguments: void
Output arguments: void
*/
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void GetFlux(void)
{
unsigned int maskpos = 0x8000, maskneg = 0x7FFF, maskbit = 0x8000;
unsigned int value = 0, masknum = 0;
value = ParkParm.qId; //get current torque value
masknum = value & maskbit;
if(masknum == 0)
{
value = value | maskpos;
}
else
{
value = value & maskneg;
}
Fluxbuf = value; //get current torque value
//load CAN bus buffer
send_buffer3[0] = 0x03;
send_buffer3[1] = (unsigned char)Fluxbuf;
Fluxbuf = Fluxbuf >> 8;
send_buffer3[2] = (unsigned char)Fluxbuf;
if(send_buffer1[2] == 0x00)
{
send_buffer1[2] = 0xFF; //bit stuffing for LABVIEW application
}
}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
Function: GetPID Parms
Description: This function will recieve CAN messages and parse through them
to get the PID parameters. The parameters are being sent 9 at a time with
a short delay between them. This function also handles a timeout and corrupted
transmissions.
Input arguments: void
Output arguments: void
*/
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void GetqPIDParms(void)
{
int buf = 0, dummy1, dummy2;
unsigned char chksum = 0;
if(timeout1 == 1) //if a timeout situation is occurring
{
timeout2++; //increment timer
}
if(timeout2 == 0xFFFF) //if a timeout has occurred
{
timeout1 = 0; //reset timers
timeout2 = 0;
time_flag = 1; //set flag to indicate a timeout
}
rx_ready = C1RX0CONbits.RXFUL;
if(rx_ready == 1)
{
timeout1 = 1; //if a packet was received then start timer
CAN1ReceiveMessage(receive_buffer, 0x08, 0x00); //receive packet
//put packet out on UART for trouble shooting
U1TXREG = receive_buffer[0];
Wait(0x00FF);
U1TXREG = receive_buffer[1];
Wait(0x00FF);
U1TXREG = receive_buffer[2];
Wait(0x00FF);
U1TXREG = receive_buffer[3];
Wait(0x00FF);
C1RX0CONbits.RXFUL = 0; //clear CAN bus receive flag
chksum = receive_buffer[0] + receive_buffer[1] + receive_buffer[2]; //calculate checksum
buf = buf | (int)receive_buffer[2]; //Make CAN bytes into an int
buf = buf << 8;
buf = buf | (int)receive_buffer[1];
//Load qPID coefficients buffer for EEPROM storage later
if(chksum == receive_buffer[3])
{
switch (receive_buffer[0]) //check ID byte
{
case 0x0B:
{
qPID[0] = buf;
chk1 = 1;
chk2++;
break;
}
case 0x0C:
{
qPID[1] = buf;
chk1 = 2;
chk2++;
break;
}
case 0x0D:
{
qPID[2] = buf;
chk1 = 3;
chk2++;
break;
}
case 0x0E:
{
qPID[3] = buf;
chk1 = 4;
chk2++;
break;
}
case 0x0F:
{
qPID[4] = buf;
chk1 = 5;
chk2++;
break;
}
case 0x10:
{
qPID[5] = buf;
chk1 = 6;
chk2++;
break;
}
case 0x11:
{
qPID[6] = buf;
chk1 = 7;
chk2++;
break;
}
case 0x12:
{
qPID[7] = buf;
chk1 = 8;
chk2++;
break;
}
case 0x13:
{
qPID[8] = buf;
chk1 = 9;
chk2++;
timeout1 = 0;
break;
}
default:
{
