/** 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 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;
}
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 lcdcmd(unsigned char value)
{
PORTE = value; // write whole port E
//PORTWrite(IOPORT_E) = value;
//PORTWrite(IOPORT_E,BIT_0|BIT_1|BIT_2|BIT_3|BIT_4|BIT_5|BIT_6|BIT_7) = value;
PORTClearBits(IOPORT_D, BIT_5);
PORTSetBits(IOPORT_D, BIT_4);
msdelay(100);
PORTClearBits(IOPORT_D, BIT_4);
return;
}
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);
}
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;
}
/**
* 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;
}
}
/**
* 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 OledDisplayOff()
{
PORTClearBits(prtDataCmd, bitDataCmd);
Spi2PutByte(cmdOledDisplayOff);
}
void LcdInit(void) {
PORTSetPinsDigitalOut(IOPORT_E, PE_LCD_DATA);
PORTSetPinsDigitalOut(IOPORT_G, PG_LCD_RS |
PG_LCD_RW |
PG_LCD_E);
PORTClearBits(IOPORT_G, PG_LCD_E);
// clear the FIFO
lcdFifoWr = 0;
lcdFifoRd = 0;
lcdDelay = 0;
// Initialize a 4 line LCD
LcdFifoCmd(LCD_CMD_FSET);
LcdFifoCmd(LCD_CMD_FSET);
LcdFifoCmd(LCD_CMD_4LINE);
LcdFifoCmd(LCD_CMD_FCLR);
LcdFifoCmd(LCD_CMD_DISP_ON);
LcdFifoCmd(LCD_CMD_CLEAR);
LcdFifoCmd(LCD_CMD_WR_DATA);
LcdNewState(LCD1_VERSION);
LcdButtonWaitClear();
lcdDemoRate = 40;
lcdDemoPause = 5000;
} // LcdInit()
/**
* Set the LCD to display pixel values as the opposite of how they are actually stored in NVRAM. So
* pixels set to black (0) will display as white, and pixels set to white (1) will display as black.
*/
void OledDriverSetDisplayInverted(void)
{
// Set the OLED into command mode.
PORTClearBits(OLED_DRIVER_MODE_PORT, OLED_DRIVER_MODE_BIT);
_Spi2Put(OLED_COMMAND_DISPLAY_INVERTED);
}
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;
}
}
/**
* Set the LCD to display pixel values normally, where a 1 indicates white and a 0 indicates black.
* This is the default operating mode of the LCD and the mode it starts up in.
*/
void OledDriverSetDisplayNormal(void)
{
// Set the OLED into command mode.
PORTClearBits(OLED_DRIVER_MODE_PORT, OLED_DRIVER_MODE_BIT);
_Spi2Put(OLED_COMMAND_DISPLAY_NORMAL);
}
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;
}
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);
}
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 Disp_Init(){
PORTClearBits(IOPORT_C , BIT_3|BIT_2|BIT_1);
PORTClearBits(IOPORT_E , BIT_2|BIT_3|BIT_4|BIT_5|BIT_6|BIT_7);
PORTClearBits(IOPORT_G , BIT_12|BIT_13|BIT_14|BIT_15);
PORTSetPinsDigitalOut(IOPORT_C , BIT_3|BIT_2|BIT_1);
PORTSetPinsDigitalOut(IOPORT_E , BIT_2|BIT_3|BIT_4|BIT_5|BIT_6|BIT_7);
PORTSetPinsDigitalOut(IOPORT_G , BIT_12|BIT_13|BIT_14|BIT_15);
mPORTCOpenDrainOpen(BIT_3|BIT_2|BIT_1);
mPORTEOpenDrainOpen(BIT_2|BIT_3|BIT_4|BIT_5|BIT_6|BIT_7);
mPORTGOpenDrainOpen(BIT_12|BIT_13|BIT_14|BIT_15);
WR=0; //WR
PSB=1; //PSB
}
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);
}
/*
* Clears the specified hbridge's direction pin
*/
static void clear_direction(uint8_t hbridge_id)
{
enum motor_list motor = (enum motor_list)hbridge_id;
if (motor < NUM_MOTORS)
{
PORTClearBits(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);
}
}
/**
* 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);
}
/********************************************************************
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;
}
}
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
}
void OledDevTerm()
{
/* Send the Display Off command.
*/
Spi2PutByte(cmdOledDisplayOff);
/* Turn off VCC
*/
PORTSetBits(prtVbatCtrl, bitVbatCtrl);
delay(100);
/* Turn off VDD
*/
PORTClearBits(prtVddCtrl, bitVddCtrl);
}
/** 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;
}
int main(void) {
MainInit();
I2C1init();
RtcInit();
LcdInit();
DataLogInit();
StringInit();
Mct485Init();
FieldInit();
Ads1115Init();
Ads1244Init();
USBInit();
RtuInit();
AdcInit();
TC74Init();
// enable multi-vector interrupts
INTEnableSystemMultiVectoredInt();
MainDelay(50);
DataLogDateTime(DLOG_SFC_POWERUP);
// init param after interrupts are enabled
ParamInit();
// wait to init desiccant until after ParamInit
DesiccantInit();
string[0].mct[0].chan[4] = 0x7FFF;
//
// Begin Main Loop
//
for (;;) {
if (test == 1) {
test = 0;
FieldNewState((FIELD_STATE_m)t1);
}
USBUpdate(); // called as often as possible
//sysTickEvent every ms
if (sysTickEvent) {
sysTickEvent = 0;
sysTicks++;
UsbTimeoutUpdate();
LcdUpdate();
// fill time before dropping LCD_E
if (sysTicks >= 1000) {
sysSec++;
sysTicks = 0;
mPORTDToggleBits(PD_LED_HEARTBEAT);
// These Updates are
// called once a second
//TODO if any of these are long, we could split them on separate milliseconds.
DesiccantUpdate();
AdcUpdate();
TC74Update();
}// end 1 Hz
else if (sysTicks == 250) {
mPORTDToggleBits(PD_LED_HEARTBEAT);
}
else if (sysTicks == 500) {
mPORTDToggleBits(PD_LED_HEARTBEAT);
}
else if (sysTicks == 750) {
mPORTDToggleBits(PD_LED_HEARTBEAT);
}
// Complete LcdUpdate() by dropping LCD_E)
PORTClearBits(IOPORT_G, PG_LCD_E);
// These Updates called once each millisecond
RtcUpdate();
I2C1update();
StringUpdate();
Mct485Update();
FieldUpdate();
RtuUpdate();
DessicantFanPWMupdate();
} // if (sysTickEvent)
} // for (;;)
} // main()
int main(void)
{
bool ret_val = false;
int i = 0;
char message[20];
my_delay_timer delay_timer_ref = my_delay_timer::get_instance();
delay_timer_ref.init(SYS_CLOCK / PB_DIV);
INTEnableSystemMultiVectoredInt();
my_I2C_handler i2c_driver = my_I2C_handler::get_instance();
ret_val = i2c_driver.init(I2C2, SYS_CLOCK / PB_DIV, DESIRED_I2C_FREQ_1KHZ);
if (!ret_val)
{
WOOP_WOOP_WOOP();
}
ret_val = i2c_driver.CLS_init();
if (!ret_val)
{
WOOP_WOOP_WOOP();
}
ret_val = i2c_driver.CLS_write_to_line("CLS initialized", 1);
if (!ret_val)
{
WOOP_WOOP_WOOP();
}
ret_val = i2c_driver.temp_init();
if (!ret_val)
{
WOOP_WOOP_WOOP();
}
// turn an LED on and off for a little light show because...reasons
PORTSetPinsDigitalOut(IOPORT_B, BIT_10);
PORTClearBits(IOPORT_B, BIT_10);
// loop forever
i = 0;
bool LED_is_on = false;
float temp = 0.0f;
while(1)
{
ret_val = i2c_driver.temp_read(&temp, true);
if (ret_val)
{
snprintf(message, CLS_LINE_SIZE, "temp = '%.3f'", temp);
}
else
{
snprintf(message, CLS_LINE_SIZE, "temp = XXXX");
}
i2c_driver.CLS_write_to_line(message, 1);
snprintf(message, CLS_LINE_SIZE, "i = '%d'", i);
i2c_driver.CLS_write_to_line(message, 2);
if (LED_is_on)
{
PORTClearBits(IOPORT_B, BIT_10);
LED_is_on = false;
}
else
{
PORTSetBits(IOPORT_B, BIT_10);
LED_is_on = true;
}
i++;
delay_timer_ref.delay_ms(200);
}
}
/********************************************************************
* 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);
}
}
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()
