void MOTtaskInit() {
//make sure the motors are disabled
PORTSetPinsDigitalOut(MOT_RST_IOPORT, MOT_RST_BIT);
PORTSetBits(MOT_RST_IOPORT, MOT_RST_BIT);
//set all PWM ports high
PORTSetPinsDigitalOut(PWM_1_IOPORT, PWM_1_BIT);
PORTSetBits(PWM_1_IOPORT, PWM_1_BIT);
PORTSetPinsDigitalOut(PWM_2_IOPORT, PWM_2_BIT);
PORTSetBits(PWM_2_IOPORT, PWM_2_BIT);
PORTSetPinsDigitalOut(PWM_3_IOPORT, PWM_3_BIT);
PORTSetBits(PWM_2_IOPORT, PWM_3_BIT);
PORTSetPinsDigitalOut(PWM_4_IOPORT, PWM_4_BIT);
PORTSetBits(PWM_2_IOPORT, PWM_4_BIT);
//start the MOT main task
if (xTaskCreate(MOTtask, /* The function that implements the task. */
(signed char *) "Motor Task", /* The text name assigned to the task - for debug only as it is not used by the kernel. */
MOT_TASK_STACK_SIZE, /* The size of the stack to allocate to the task. */
(void *) 0, /* The parameter passed to the task. */
MOT_TASK_PRIORITY, /* The priority assigned to the task. */
NULL) /* The task handle is not required, so NULL is passed. */
!= pdPASS) {
DebugPrint( "MOT Task Fail");
while(1);
}
}
void initialiseIO()
{
//Led Fault
mPORTBSetPinsDigitalOut(BIT_8);
PORTSetBits(IOPORT_B, BIT_8);
//Led Alert
mPORTBSetPinsDigitalOut(BIT_9);
PORTSetBits(IOPORT_B, BIT_9);
//Relais 1
mPORTDSetPinsDigitalOut(BIT_1);
PORTSetBits(IOPORT_D, BIT_1);
//Relais 2
mPORTDSetPinsDigitalOut(BIT_2);
PORTSetBits(IOPORT_D, BIT_2);
//Address
mPORTESetPinsDigitalIn(BIT_0);
mPORTESetPinsDigitalIn(BIT_1);
mPORTESetPinsDigitalIn(BIT_2);
mPORTESetPinsDigitalIn(BIT_3);
mPORTESetPinsDigitalIn(BIT_4);
mPORTESetPinsDigitalIn(BIT_5);
mPORTESetPinsDigitalIn(BIT_6);
mPORTESetPinsDigitalIn(BIT_7);
}
void OledPutBuffer(int cb, BYTE * rgbTx)
{
int ib;
int bit;
BYTE bVal;
for(ib = 0; ib < cb; ib++) {
bVal = *rgbTx++;
for(bit = 0; bit < 8; bit++) {
/* Check if MSB is 1 or 0 and set MOSI pin accordingly
*/
if(bVal & 0x80)
PORTSetBits(prtMosi, bitMosi);
else
PORTClearBits(prtMosi, bitMosi);
/* Lower the clock line
*/
PORTClearBits(prtSck, bitSck);
/* Shift byte being sent to the left by 1
*/
bVal <<= 1;
/* Raise the clock line
*/
PORTSetBits(prtSck, bitSck);
}
}
}
BYTE Spi2PutByte(BYTE bVal)
{
int bit;
BYTE bRx;
for(bit = 0; bit < 8; bit++) {
/* Check if MSB is 1 or 0 and set MOSI pin accordingly
*/
if(bVal & 0x80)
PORTSetBits(prtMosi, bitMosi);
else
PORTClearBits(prtMosi, bitMosi);
/* Lower the clock line
*/
PORTClearBits(prtSck, bitSck);
/* Shift byte being sent to the left by 1
*/
bVal <<= 1;
/* Raise the clock line
*/
PORTSetBits(prtSck, bitSck);
}
return bRx;
}
/** Soft SPI send */
void spiSend(uint8_t data) {
for (uint8_t i = 0; i < 8; i++) {
if(data & 0X80) {
PORTSetBits(prtSDO, bnSDO);
}
else
{
PORTClearBits(prtSDO, bnSDO);
}
PORTClearBits(prtSCK, bnSCK);
asm("nop");
asm("nop");
asm("nop");
data <<= 1;
PORTSetBits(prtSCK, bnSCK);
}
// hold SCK high for a few ns
asm("nop");
asm("nop");
asm("nop");
asm("nop");
PORTClearBits(prtSCK, bnSCK);
}
void
OledHostInit()
{
// unsigned int tcfg; // ALA unused
/* Initialize SPI port 1.
*/
SPI1CON = 0;
SPI1BRG = 15; //8Mhz, with 80Mhz PB clock
SPI1STATbits.SPIROV = 0;
SPI1CONbits.CKP = 1;
SPI1CONbits.MSTEN = 1;
SPI1CONbits.ON = 1;
/* Make power control pins be outputs with the supplies off
*/
PORTSetBits(prtVddCtrl, bitVddCtrl);
PORTSetBits(prtVbatCtrl, bitVbatCtrl);
PORTSetPinsDigitalOut(prtVddCtrl, bitVddCtrl); //VDD power control (1=off)
PORTSetPinsDigitalOut(prtVbatCtrl, bitVbatCtrl); //VBAT power control (1=off)
/* Make the Data/Command select, Reset, and SPI CS pins be outputs.
*/
PORTSetBits(prtDataCmd, bitDataCmd);
PORTSetPinsDigitalOut(prtDataCmd, bitDataCmd); //Data/Command# select
PORTSetBits(prtReset, bitReset);
PORTSetPinsDigitalOut(prtReset, bitReset);
PORTSetBits(prtSelect, bitSelect);
PORTSetPinsDigitalOut(prtSelect, bitSelect);
}
void lcddata(unsigned char value)
{
PORTE = value; // write whole port E
//mPORTEWrite = value;
//PORTWrite(IOPORT_E,BIT_0|BIT_1|BIT_2|BIT_3|BIT_4|BIT_5|BIT_6|BIT_7) = value1;
PORTSetBits(IOPORT_D, BIT_5);
PORTSetBits(IOPORT_D, BIT_4);
msdelay(100);
PORTClearBits(IOPORT_D,BIT_4);
return;
}
void
OledUpdate()
{
int ipag;
// int icol; // ALA unused
BYTE * pb;
pb = rgbOledBmp;
for (ipag = 0; ipag < cpagOledMax; ipag++) {
PORTClearBits(prtDataCmd, bitDataCmd);
/* Set the page address
*/
Spi1PutByte(0x22); //Set page command
Spi1PutByte(ipag); //page number
/* Start at the left column
*/
Spi1PutByte(0x00); //set low nybble of column
Spi1PutByte(0x10); //set high nybble of column
PORTSetBits(prtDataCmd, bitDataCmd);
/* Copy this memory page of display data.
*/
OledPutBuffer(ccolOledMax, pb);
pb += ccolOledMax;
}
}
uint8_t PmodHB5ChangeDirection(HBRIDGE *hBridge)
{
if(hBridge->newDirection != hBridge->currentDirection)
{
PmodHB5SetDCPWMDutyCycle(0,hBridge->ocChannel);
if(hBridge->rpm == 0)
{
if(hBridge->newDirection == PMOD_HB5_DIR_FWD)
{
PORTSetBits(hBridge->directionPort,hBridge->directionPortBit);
}
else
{
PORTClearBits(hBridge->directionPort,hBridge->directionPortBit);
}
hBridge->currentDirection = hBridge->newDirection;
return 1;
}
else
{
return 0;
}
}
return 1;
}
uint8_t UNIT_PmodHB5SetDCPWMDutCycle(UART_MODULE uartID)
{
uint8_t dutyCycle = 0;
PmodHB5SetDCPWMDutyCycle(dutyCycle,hBridge.ocChannel) ;
UARTPutS("\n\rEXECUTING TEST =>UNIT_PmodHB5SetDCPWMDutyCycle\r\n",uartID);
do
{
UARTPutS("Enter a duty cycle percentage between 0 and 100 =>",uartID);
dutyCycle = getIntegerFromConsole(uartID);
if(dutyCycle > 100)
{
UARTPutS("Invalid Entry\r\n",uartID);
}
}while(dutyCycle > 100);
PORTSetBits(IOPORT_D,BIT_7); //set direction forward
PmodHB5SetDCPWMDutyCycle(PR2 * dutyCycle/100,hBridge.ocChannel);
if(OC2RS == PR2 * dutyCycle/100)
{
return 1;
}
else
{
return 0;
}
}
void
OledPutBuffer(int cb, BYTE * rgbTx)
{
int ib;
BYTE bTmp;
/* Bring the slave select line low
*/
PORTClearBits(prtSelect, bitSelect);
/* Write/Read the data
*/
for (ib = 0; ib < cb; ib++) {
/* Wait for transmitter to be ready
*/
while (SPI1STATbits.SPITBE == 0);
/* Write the next transmit byte.
*/
SPI1BUF = *rgbTx++;
/* Wait for receive byte.
*/
while (SPI1STATbits.SPIRBF == 0);
bTmp = SPI1BUF;
}
/* Bring the slave select line high
*/
PORTSetBits(prtSelect, bitSelect);
}
BYTE
Spi1PutByte(BYTE bVal)
{
BYTE bRx;
/* Bring the slave select line low
*/
PORTClearBits(prtSelect, bitSelect);
/* Wait for transmitter to be ready
*/
while (SPI1STATbits.SPITBE == 0);
/* Write the next transmit byte.
*/
SPI1BUF = bVal;
/* Wait for receive byte.
*/
while (SPI1STATbits.SPIRBF == 0);
/* Put the received byte in the buffer.
*/
bRx = SPI1BUF;
/* Bring the slave select line high
*/
PORTSetBits(prtSelect, bitSelect);
return bRx;
}
/**
* Update the display with the contents of rgb0ledBmp.
*/
void OledDriverUpdateDisplay(void)
{
uint8_t *pb = rgbOledBmp;
int page;
for (page = 0; page < OLED_DRIVER_PAGES; page++) {
// Set the LCD into command mode.
PORTClearBits(OLED_DRIVER_MODE_PORT, OLED_DRIVER_MODE_BIT);
// Set the desired page.
_Spi2Put(OLED_COMMAND_SET_PAGE);
_Spi2Put(page);
// Set the starting column back to the origin.
_Spi2Put(OLED_COMMAND_SET_DISPLAY_LOWER_COLUMN_0);
_Spi2Put(OLED_COMMAND_SET_DISPLAY_UPPER_COLUMN_0);
// Return the LCD to data mode.
PORTSetBits(OLED_DRIVER_MODE_PORT, OLED_DRIVER_MODE_BIT);
// Finally write this entire column to the OLED.
// SpiChnPutS()
_OledPutBuffer(OLED_DRIVER_PIXEL_COLUMNS, pb);
pb += OLED_DRIVER_PIXEL_COLUMNS;
}
}
void LcdUpdate(void) {
if (lcdButtonDelay) {
lcdButtonDelay--;
}
else {
LcdButtonUpdate();
lcdButtonDelay = 7;
}
// redraw when lcdRedraw is zero
if (lcdRedraw) {
if (lcdRedraw != 0xFFFF) {
lcdRedraw--;
}
}
else {
LcdDrawState();
}
// Some commands require a 2msec delay
if (lcdDelay) {
lcdDelay--;
return;
}
if (lcdFifoRd != lcdFifoWr) {
// 100nsec setup time before ECLK
if (lcdFifo[lcdFifoRd] & 0x100) {
// data byte
PORTSetBits(IOPORT_G, PG_LCD_RS);
}
else {
// Command byte
PORTClearBits(IOPORT_G, PG_LCD_RS);
lcdDelay = 1;
}
PORTClearBits(IOPORT_G, PG_LCD_RW);
PORTClearBits(IOPORT_E, PE_LCD_DATA);
PORTSetBits(IOPORT_E, (byte)lcdFifo[lcdFifoRd]);
// 300nsec high time for ECLK
lcdFifoRd++;
if (lcdFifoRd >= LCD_FIFO_SIZE) {
lcdFifoRd = 0;
}
PORTSetBits(IOPORT_G, PG_LCD_E);
}
} // LcdUpdate()
void setLEDstate(unsigned int led, unsigned int state) {
//if the state should be on (1) Turn on the LED
if (state){
PORTSetBits(IOPORT_G, led);
}
// else clear the LEDs
else
PORTClearBits(IOPORT_G, led);
}
/**
* Initialize the PIC32MX to communicate with the UG-23832HSWEG04 OLED display through the SSD1306
* display controller.
*/
void OledHostInit(void)
{
// Open SPI2 as a master in 1-byte mode running at 10MHz.
// The peripheral bus is running at 10Mhz, and we want a 10MHz SPI bus clock.
int pbClkDiv = 20000000 / 10000000;
SpiChnOpen(SPI_CHANNEL2, SPI_OPEN_MSTEN | SPI_OPEN_CKP_HIGH | SPI_OPEN_MODE8, pbClkDiv);
// Set RF4-6 as digital outputs for controlling data/command selection, logic power, and display
// power. They're all initialized high beforehand, because that disables power.
PORTSetBits(IOPORT_F, OLED_DRIVER_CNTLR_POWER_BIT | OLED_DRIVER_OLED_POWER_BIT | OLED_DRIVER_MODE_BIT);
PORTSetPinsDigitalOut(OLED_DRIVER_MODE_PORT, OLED_DRIVER_MODE_BIT); // RF4 sets whether the next SPI byte is a data or command byte.
PORTSetPinsDigitalOut(OLED_DRIVER_CNTLR_POWER_PORT, OLED_DRIVER_CNTLR_POWER_BIT); // RF5 controls power to the SSD1306 display controller.
PORTSetPinsDigitalOut(OLED_DRIVER_OLED_POWER_PORT, OLED_DRIVER_OLED_POWER_BIT); // RF6 controls power to the UG-23832HSWEG04 OLED display.
// Set RG9 as a digital output, tied to the reset pin on the SG1306 controller, low => reset.
PORTSetBits(OLED_DRIVER_RESET_PORT, OLED_DRIVER_RESET_BIT);
PORTSetPinsDigitalOut(OLED_DRIVER_RESET_PORT, OLED_DRIVER_RESET_BIT);
}
/*
* Sets the specified hbridge's direction pin
*/
static void set_direction(uint8_t hbridge_id)
{
enum motor_list motor = (enum motor_list)hbridge_id;
if (motor < NUM_MOTORS)
{
PORTSetBits(Motors[hbridge_id].dir_port,
Motors[hbridge_id].dir_bit);
}
}
/********************************************************************
Funciton: void CUPLDConfigure(CPLD_CONFIGURATION configuration
********************************************************************/
void CPLDSetGraphicsConfiguration(CPLD_GFX_CONFIGURATION configuration)
{
if(configuration == CPLD_GFX_CONFIG_16BIT)
{
PORTSetBits(IOPORT_G, BIT_14);
}else
{
PORTClearBits(IOPORT_G, BIT_14);
}
}
/** Soft SPI receive */
uint8_t spiRec(void) {
uint8_t data = 0;
// output pin high - like sending 0XFF
PORTSetBits(prtSDO, bnSDO);
for (uint8_t i = 0; i < 8; i++) {
PORTSetBits(prtSCK, bnSCK);
data <<= 1;
// adjust so SCK is nice
asm("nop");
asm("nop");
if (PORTReadBits(prtSDI,bnSDI)) data |= 1;
PORTClearBits(prtSCK, bnSCK);
}
return data;
}
void OledHostTerm()
{
/* Make the Data/Command select, Reset, and SPI CS pins be inputs.
*/
PORTSetBits(prtDataCmd, bitDataCmd);
PORTSetPinsDigitalIn(prtDataCmd, bitDataCmd); //Data/Command# select
PORTSetBits(prtReset, bitReset);
PORTSetPinsDigitalIn(prtReset, bitReset);
/* Make power control pins be inputs. The pullup resistors on the
** board will ensure that the power supplies stay off.
*/
PORTSetBits(prtVddCtrl, bitVddCtrl);
PORTSetBits(prtVbatCtrl, bitVbatCtrl);
PORTSetPinsDigitalIn(prtVddCtrl, bitVddCtrl); //VDD power control (1=off)
PORTSetPinsDigitalIn(prtVbatCtrl, bitVbatCtrl); //VBAT power control (1=off)
/* Turn SPI port 2 off.
*/
#if defined (_BOARD_UNO_)
SPI2CON = 0;
#elif defined (_BOARD_MEGA_)
PORTSetBits(prtSck, bitSck);
PORTSetBits(prtMosi, bitMosi);
PORTSetPinsDigitalIn(prtSck, bitSck);
PORTSetPinsDigitalIn(prtMosi, bitMosi);
#endif
}
void
OledDevInit()
{
/* We're going to be sending commands, so clear the Data/Cmd bit
*/
PORTClearBits(prtDataCmd, bitDataCmd);
/* Start by turning VDD on and wait a while for the power to come up.
*/
PORTClearBits(prtVddCtrl, bitVddCtrl);
DelayMs(1);
/* Display off command
*/
Spi1PutByte(0xAE);
/* Bring Reset low and then high
*/
PORTClearBits(prtReset, bitReset);
DelayMs(1);
PORTSetBits(prtReset, bitReset);
/* Send the Set Charge Pump and Set Pre-Charge Period commands
*/
Spi1PutByte(0x8D);
Spi1PutByte(0x14);
Spi1PutByte(0xD9);
Spi1PutByte(0xF1);
/* Turn on VCC and wait 100ms
*/
PORTClearBits(prtVbatCtrl, bitVbatCtrl);
DelayMs(100);
/* Send the commands to invert the display.
*/
Spi1PutByte(0xA1); //remap columns
Spi1PutByte(0xC8); //remap the rows
/* Send the commands to select sequential COM configuration
*/
Spi1PutByte(0xDA); //set COM configuration command
Spi1PutByte(0x20); //sequential COM, left/right remap enabled
/* Send Display On command
*/
Spi1PutByte(0xAF);
}
void IO_M1_SetDirection(unsigned char dir)
{
#ifdef _TARGET_440H
#else
if(dir==0)
{
PORTClearBits(IO_Motor_dir_1);
}
else
{
PORTSetBits(IO_Motor_dir_1);
}
#endif
}
/********************************************************************
Section: Code
********************************************************************/
inline void SetSPIChannel(CPLD_SPI_CONFIGURATION channel_config)
{
switch(channel_config)
{
case CPLD_SPI2A:
PORTSetBits(IOPORT_G, BIT_12);
break;
case CPLD_SPI3A:
PORTClearBits(IOPORT_G, BIT_12);
break;
}
}
/**
* Disable the Oled display before power-off. This means powering it up, sending the display off
* command, and finally disabling Vbat.
*/
void OledDriverDisableDisplay(void)
{
// Set the OLED into command mode.
PORTClearBits(OLED_DRIVER_MODE_PORT, OLED_DRIVER_MODE_BIT);
// Power on the OLED display logic, waiting for 1ms for it to start up.
PORTClearBits(OLED_DRIVER_CNTLR_POWER_PORT, OLED_DRIVER_CNTLR_POWER_BIT);
_DelayMs(1);
// Send the display off command.
_Spi2Put(OLED_COMMAND_DISPLAY_OFF);
// And finally power off the display, giving it 100ms to do so.
PORTSetBits(OLED_DRIVER_OLED_POWER_PORT, OLED_DRIVER_OLED_POWER_BIT);
_DelayMs(100);
}
void LED_On (CPU_INT08U led)
{
switch (led) {
case 0:
#ifdef _TARGET_440H
PORTClearBits(IOPORT_D, BIT_6 | BIT_7);
PORTClearBits(IOPORT_F, BIT_0 | BIT_1);
#else
//PORTSetBits(IO_LED1);
//PORTSetBits(IO_LED2);
PORTSetBits(IO_LED3);
PORTSetBits(IO_LED4);
PORTSetBits(IO_LED5);
#endif
break;
case 1:
#ifdef _TARGET_440H
PORTClearBits(IOPORT_D, BIT_7);
#else
//PORTSetBits(IO_LED1);
#endif
break;
case 2:
#ifdef _TARGET_440H
#else
//PORTSetBits(IO_LED2);
#endif
break;
case 3:
#ifdef _TARGET_440H
PORTClearBits(IOPORT_F, BIT_0);
#else
PORTSetBits(IO_LED3);
#endif
break;
case 4:
#ifdef _TARGET_440H
PORTClearBits(IOPORT_F, BIT_1);
#else
PORTSetBits(IO_LED4);
#endif
break;
case 5:
#ifdef _TARGET_440H
#else
PORTSetBits(IO_LED5);
#endif
break;
default:
break;
}
}
void OledDevTerm()
{
/* Send the Display Off command.
*/
Spi2PutByte(cmdOledDisplayOff);
/* Turn off VCC
*/
PORTSetBits(prtVbatCtrl, bitVbatCtrl);
delay(100);
/* Turn off VDD
*/
PORTClearBits(prtVddCtrl, bitVddCtrl);
}
/**
* Initialize the OLED display and driver circuitry.
*/
void OledDriverInitDisplay(void)
{
// Set the OLED into command mode.
PORTClearBits(OLED_DRIVER_MODE_PORT, OLED_DRIVER_MODE_BIT);
// Power on the display logic, waiting 1ms for it to start up.
PORTPORTClearBits(OLED_DRIVER_MODE_PORT, OLED_DRIVER_MODE_BIT);
ClearBits(OLED_DRIVER_CNTLR_POWER_PORT, OLED_DRIVER_CNTLR_POWER_BIT);
_DelayMs(1);
// Turn off the display.
_Spi2Put(OLED_COMMAND_DISPLAY_OFF);
// Toggle the reset pin.
PORTClearBits(OLED_DRIVER_RESET_PORT, OLED_DRIVER_RESET_BIT);
_DelayMs(1);
PORTSetBits(OLED_DRIVER_RESET_PORT, OLED_DRIVER_RESET_BIT);
// Enable the charge pump and
_Spi2Put(OLED_COMMAND_SET_CHARGE_PUMP);
_Spi2Put(OLED_SETTING_ENABLE_CHARGE_PUMP);
_Spi2Put(OLED_COMMAND_SET_PRECHARGE_PERIOD);
_Spi2Put(OLED_SETTING_MAXIMUM_PRECHARGE);
// Power on the display, giving it 100ms to start up.
PORTClearBits(OLED_DRIVER_OLED_POWER_PORT, OLED_DRIVER_OLED_POWER_BIT);
_DelayMs(100);
// Invert row numbering so that (0,0) is upper-right.
_Spi2Put(OLED_COMMAND_SET_SEGMENT_REMAP);
_Spi2Put(OLED_SETTING_REVERSE_ROW_ORDERING);
// Set sequential COM configuration with non-interleaved memory.
_Spi2Put(OLED_COMMAND_SET_COM_PINS_CONFIG);
_Spi2Put(OLED_SETTING_SEQUENTIAL_COM_NON_INTERLEAVED);
// And turn on the display.
_Spi2Put(OLED_COMMAND_DISPLAY_ON);
}
void
OledHostInit()
{
#if defined (_BOARD_UNO_) || defined(_BOARD_UC32_)
/* Initialize SPI port 2.
*/
SPI2CON = 0;
SPI2BRG = 4; //8Mhz, with 80Mhz PB clock
SPI2STATbits.SPIROV = 0;
SPI2CONbits.CKP = 1;
SPI2CONbits.MSTEN = 1;
SPI2CONbits.ON = 1;
#elif defined (_BOARD_MEGA_)
/* Initialize pins for bit bang SPI. The Arduino Mega boards,
** and therefore the Max32 don't have the SPI port on the same
** connector pins as the Uno. The Basic I/O Shield doesn't even
** connect to the pins where the SPI port is located. So, for
** the Max32 board we need to do bit-banged SPI.
*/
PORTSetBits(prtSck, bitSck);
PORTSetBits(prtMosi, bitMosi);
PORTSetPinsDigitalOut(prtSck, bitSck);
PORTSetPinsDigitalOut(prtMosi, bitMosi);
#else
//#error "No Supported Board Defined"
#endif
PORTSetBits(prtDataCmd, bitDataCmd);
PORTSetBits(prtVddCtrl, bitVddCtrl);
PORTSetBits(prtVbatCtrl, bitVbatCtrl);
PORTSetPinsDigitalOut(prtDataCmd, bitDataCmd); //Data/Command# select
PORTSetPinsDigitalOut(prtVddCtrl, bitVddCtrl); //VDD power control (1=off)
PORTSetPinsDigitalOut(prtVbatCtrl, bitVbatCtrl); //VBAT power control (1=off)
/* Make the RG9 pin be an output. On the Basic I/O Shield, this pin
** is tied to reset.
*/
PORTSetBits(prtReset, bitReset);
PORTSetPinsDigitalOut(prtReset, bitReset);
}
void I2C1update(void) {
if (I2C1STAT & 0x0400) {
// bus collision
i2c1Mode = I2C1_MODE_ERROR;
i2c1BusState = I2C1_BUS_ERROR;
I2CEnable(I2C1, FALSE);
}
if (i2c1Delay) {
i2c1Delay--;
return;
}
if (i2c1Mode == I2C1_MODE_IDLE) {
if (i2c1QueueRd != i2c1QueueWr) {
// show that I2C1 is busy
i2c1Mode = I2C1_MODE_WRITE;
// clear the write and read indices
i2c1Queue[i2c1QueueRd].wi = 0;
i2c1Queue[i2c1QueueRd].ri = 0;
// start I2C1 transfer
i2c1BusState = I2C1_BUS_WR_DATA;
I2C1CONSET = _I2CCON_SEN_MASK;
}
} // I2C1_IDLE
else if (i2c1Mode == I2C1_MODE_WRITE_COMPLETE) {
I2C1process();
i2c1QueueRd++;
if (i2c1QueueRd >= I2C1_QUEUE_SIZE) {
i2c1QueueRd = 0;
}
i2c1Mode = I2C1_MODE_IDLE;
} // I2C1_WRITE_COMPLETE
else if (i2c1Mode == I2C1_MODE_READ_COMPLETE) {
I2C1process();
i2c1QueueRd++;
if (i2c1QueueRd >= I2C1_QUEUE_SIZE) {
i2c1QueueRd = 0;
}
i2c1Mode = I2C1_MODE_IDLE;
} // I2C1_READ_COMPLETE
else if (i2c1Mode == I2C1_MODE_ERROR) {
// usually caused is SDA is low before start
if (!(PORTA & PA_SCL1)) {
// is clock is low, raise it
PORTSetPinsDigitalOut(IOPORT_A, PA_SCL1);
PORTSetBits(IOPORT_A, PA_SCL1);
}
else {
if (!(PORTA & PA_SDA1)) {
// if SDA is still low, try another clock pulse
PORTSetPinsDigitalOut(IOPORT_A, PA_SCL1);
PORTClearBits(IOPORT_A, PA_SCL1);
}
else {
// SDA is now high, try clearing the error
PORTSetPinsDigitalOut(IOPORT_A, PA_SDA1);
PORTClearBits(IOPORT_A, PA_SDA1);
i2c1Mode = I2C1_MODE_ERR_START;
}
}
}
else if (i2c1Mode == I2C1_MODE_ERR_START) {
PORTSetBits(IOPORT_A, PA_SDA1);
i2c1Mode = I2C1_MODE_ERR_STOP;
}
else if (i2c1Mode == I2C1_MODE_ERR_STOP) {
I2C1STAT &= ~0x400;
IFS0 &= ~0x20000000;
PORTSetPinsDigitalIn(IOPORT_A, PA_SCL1 | PA_SDA1);
I2CEnable(I2C1, TRUE);
i2c1Mode = I2C1_MODE_IDLE;
}
} // I2C1update()
/********************************************************************
* Tache: void TaskControl(void *pvParameters)
*
* Overview: Tâche responsable des différents lois de commande.
* Asservissement de position pour la direction, asservissement de
* courant (couple) pour la propulsion et supervision du niveau de
* batterie. Une fois les traitement fais, elle est responsable du
* post des grandeurs intermédaire pour le debug à la tâche d'affichage
*
* Auteur Date Commentaire
*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
* Théou Jean-Baptiste 7 avril 2011 Première version
* Descoubes hugo 16 mai 2011 vs 1.1
*******************************************************************/
void TaskControl(void *pvParameters){
/* asservissement de position */
short sDirMeasure; // Mesure de position (servomoteur). Unsigned integer sur 10 bits
short sDirConsigne; // Consigne de position (servomoteur). Unsigned integer sur 10 bits
short sDirCommand; // Commande de position (servomoteur). entier compris entre 0 et 100, rapport cyclique pour PWM
/* asservissement de courant */
float fPropMeasure; // Mesure image du courant absorbé par la MCC (propulsion).
short sPropConsigne; // Consigne de courant(propulsion). Unsigned integer sur 10 bits
short sPropCommand; // Commande de courant (propulsion). entier compris entre 0 et 100, rapport cyclique pour PWM
short sPowerMeasure; // Mesure du niveau de batterie. Unsigned integer sur 10 bits
char stateControl = PROPULSION_CONTROL;// machnie d'état pour la commande
struct_DebugPrint printData; // Données à afficher (debug)
struct_mesures measuresReceive; // Mesures reçues depuis l'ISR du timer 3
for( ;; ){
/* Récupération des grandeurs de mesures pour les lois de commande */
xQueueReceive( xQueueAcquiData, &measuresReceive, portMAX_DELAY); // fonction bloquante
/* Mise à jour mesures */
sDirMeasure = measuresReceive.sDirMeasure;
fPropMeasure = (float) measuresReceive.sCurrentMeasure / 35.0; // 35 = facteur d'échelle (capteur + conditionneur + ADC)
sPowerMeasure = measuresReceive.sBattMeasure;
/* Mise à jour mesures - Affichage par le réseau */
resultat = sDirMeasure; // Mise à jour var. globale réseau
resultat_courant = measuresReceive.sCurrentMeasure; // Mise à jour var. globale réseau
/* Mise à jour consignes */
/* Protection Vitesse*/
xSemaphoreTake(xSemaphoreVitesse,portMAX_DELAY);
{
sPropConsigne = Vitesse; // récupération var. globale réseau
}
xSemaphoreGive(xSemaphoreVitesse);
/* Protection ConsDir */
xSemaphoreTake(xSemaphoreConsDir,portMAX_DELAY);
{
sDirConsigne = ConsDir; // récupération var. globale réseau
}
xSemaphoreGive(xSemaphoreConsDir);
/* Machine d'état - tâche contrôle/commande/supervision */
switch(stateControl){
case PROPULSION_CONTROL :
/* Protection Consigne courant */
if(sPropConsigne > 100){
sPropConsigne = 100;
}
else if(sPropConsigne < 0){
sPropConsigne = 0;
}
/* Asservissement de courant (couple). Régulation PI (modèle non-linéaire) */
/* Calcul fais avec des flottant - temps de traitement long */
sPropCommand = propulsionCommand (sPropConsigne , fPropMeasure);
case PROPULSION_PWM :
if((fPropMeasure < 0.0) || (fPropMeasure > MAX_CURRENT) ){
sPropCommand = 0;
}
/* Mise à jour rapport cyclique pour module PWM propulsion */
/* Protection au niveau des erreurs de calculs */
if(sPropCommand < 10)
{
sPropCommand = 0;
}
SetDCOC4PWM(ReadPeriod3() * sPropCommand / 100 );
case DIRECTION_CONTROL :
/* Protection Consigne direction */
if(sDirConsigne > HAUT_BUTE){
sDirConsigne = HAUT_BUTE;
}
else if(sDirConsigne < BAS_BUTE){
sDirConsigne = BAS_BUTE;
}
/* Limitation sur la var globale ConsDir fait localement sur le MCU ... pour le moment ! */
/* Protection ConsDir */
xSemaphoreTake(xSemaphoreConsDir,portMAX_DELAY);
{
ConsDir = sDirConsigne;
}
xSemaphoreGive(xSemaphoreConsDir);
/* Asservissement de position. Régulation proporionnelle (modèle grandement non-linéaire) */
sDirCommand = directionCommand(sDirConsigne, sDirMeasure);
case DIRECTION_PWM :
/* Vérification butées direction */
if( sDirMeasure < HAUT_BUTE && sDirMeasure > BAS_BUTE){
if(init_ok == 0){
/* Attendre initialisation utilisateur via réseau */
sDirCommand = 0;
}
else{
/* valeur absolue et sens de rotation - PWM comprise entre 0 et 100 */
if ( sDirCommand < 0 ){
/* Sens de rotation pour le driver de bras de pont */
PORTSetBits(BROCHE_DIR_DIRECTION);
sDirCommand = -sDirCommand;
}
else{
/* Sens de rotation pour le driver de bras de pont */
PORTClearBits(BROCHE_DIR_DIRECTION);
}
/* Protection et limitation Commande */
if(sDirCommand < 20){
sDirCommand = 0; // Pas de commande si dans dead zone
}
else if(sDirCommand < 30){
sDirCommand = 40; // compensation dead zone
}
else if (sDirCommand > 100){
sDirCommand = 100; // limitation PWM
}
}
}
else{
sDirCommand = 0;
}
/* Mise à jour rapport cyclique pour module PWM direction */
SetDCOC2PWM(ReadPeriod3() * sDirCommand / 100 );
case POWER_SUPERVIOR :
break;
default:
break;
}
/* Post les valeurs à afficher pour le debug vers la tâche d'affichage */
printData.commandeDir = sDirCommand;
printData.mesureDir = sDirMeasure;
printData.consigneDir = sDirConsigne;
printData.consigneMove = (float) sPropConsigne;
printData.mesureMove = fPropMeasure;
printData.commandeMove = sPropCommand;
printData.mesurePower = sPowerMeasure;
xQueueSend(xQueueDebugPrint, &printData, 0);
}
}
