uchar usbFunctionRead(uchar *data, uchar len)
{
pwmSet(CHANNEL2, 0, 0);
pwmSet(CHANNEL1, 0, 0);
pwmSet(CHANNEL0, 0, 0);
return 1;
}
void usbFunctionWriteOut(uchar *data, uchar len)
{
int i;
// Check if this is a new data block
if (!to_write)
{
buffer_transform = data[0];
to_write = (((int) data[1]) << 8) | data[2];
dropper_index = 0;
}
// Copy and transform data
for (i = 0; i < len; i++)
{
switch (buffer_transform)
{
case 1:
buffer[buffer_write_index] = data[i];
to_write--;
buffer_write_index++;
break;
case 2:
buffer[buffer_write_index] = data[i] ^ 0xff;
to_write--;
buffer_write_index++;
break;
case 3:
buffer[buffer_write_index] = data[i] ^ 0x55;
to_write--;
buffer_write_index++;
break;
case 4:
if (dropper_index != 2)
{
buffer[buffer_write_index] = data[i] ^ 0x55;
buffer_write_index++;
dropper_index++;
} else dropper_index = 0;
to_write--;
break;
default:
pwmSet(CHANNEL0, 255, 0);
pwmSet(CHANNEL1, 0, 0);
pwmSet(CHANNEL2, 0, 0);
}
}
// When write is complete then start reading
if (!to_write)
{
to_read = buffer_write_index;
}
}
void wakeTimer()
{
stopTimerCount = 0;
if (state == state_Sleep) {
startShowTime();
tick_timer_reset(delay_Second, TICK_SECOND);
tick_timer_reset(delay_Update, 1);
pwmSet(pwmRunning);
updateTimer();
} else
pwmSet(pwmRunning);
}
/**
* Called to setup a USB request. In most cases this also handles the whole
* USB request. Only when a larger data block is to written to the memory
* then it passes the control to the usbFunctionWrite function.
*/
usbMsgLen_t usbFunctionSetup(uchar setupData[8])
{
#define usbData ((usbRequest_t *) setupData)
switch (usbData->bRequest)
{
case 0x14:
return 0xff;
/* Go into flash mode */
case 0x2:
restart--;
memory[0] &= 0xbf;
break;
case 0x1:
case 0xb0:
if (usbData->bmRequestType & USBRQ_DIR_DEVICE_TO_HOST)
{
/* Read mode */
usbMsgPtr = &buffer[usbData->wIndex.word];
return usbData->wLength.word > buffer_size ? buffer_size : usbData->wLength.word;
}
else
{
/* Write mode */
usbWriteIndex = usbData->wIndex.word;
usbWriteLength = usbData->wLength.word;
buffer_size = usbData->wLength.word;
return USB_NO_MSG;
}
break;
case 0xa8:
if (!(usbData->bmRequestType & USBRQ_DIR_DEVICE_TO_HOST))
{
pwmSet(CHANNEL0, 255, 0);
pwmSet(CHANNEL1, 255, 0);
pwmSet(CHANNEL2, 255, 0);
restart--;
return 0;
}
break;
default:
break;
}
return 0;
}
static void reenumerate(uint16_t delay)
{
usbDeviceDisconnect();
do
{
_delay_ms(1);
}
while(--delay);
usbDeviceConnect();
pwmSet(CHANNEL0, 0, 0);
pwmSet(CHANNEL1, 0, 0);
pwmSet(CHANNEL2, 255, 0);
}
/*
* Called every seconds to update the visuals
*/
void second_timer_callback(struct tick_t *t)
{
t->delay = TICK_SECOND;
if (state & state_IncrementTime) {
pwmSet(pwmRunning);
decimalInc();
} else {
if (tick_timer_fired(delay_StopFade)) {
stopTimerCount++;
if (stopTimerCount >= STANDBY_DELAY) {
if (OCR0A < 0xff) {
if ((stopTimerCount & 0xf) == 0) // very gradualy fade out to zero (notch every 8 secs)
OCR0A++;
} else
sleepTimer(); // this will stop the one second timer
} else {
if (OCR0A != pwmStopped) {
if (OCR0A > pwmStopped)
OCR0A--;
else
OCR0A++;
}
}
}
}
updateTimerDisplay();
}
void sleepTimer()
{
state = state_Sleep;
tick_timer_reset(delay_Second, 0);
tick_timer_reset(delay_Update, 0);
pwmSet(0xff); // stop the LEDs completely
}
/**
* @fn void traitementMesures(void* pvParameters)
* @brief Traite les données reçues par le SPI
* @brief Se réveille à chaque tick (réglé à 2kHz dans FreeRTOSConfig.h) (A MODIFIER)
* @param pvParameters Pointeur sur un paramètre passé à la tâche
*/
void traitementMesures( void* pvParameters ) {
unsigned int compteurGPIO = 0;
portTickType xLastWakeTime;
const portTickType xFrequency = 640;
// boucle d'excecution
for(;;) {
xLastWakeTime = xTaskGetTickCount();
mesure[0][pMesure] = ads7885Pic32Read( CHN_SPI,1); //data_IL;
mesure[1][pMesure] = ads7885Pic32Read( CHN_SPI,2); //data_VI;
mesure[2][pMesure] = ads7885Pic32Read( CHN_SPI,3); //data_VO;
// Commande
if (flagAuto != 0) {
// Calcul
commande[pCommande] = execute_rst( ref, mesure[2][pMesure] );
pwmSet(commande[pCommande]);
} else {
pwmSet(pwm);
}
// Incrémentation
pMesure++;
if (pMesure >= TAILLE_MESURE)
pMesure = 0;
//Prise en charge de la DEL
compteurGPIO++;
if (compteurGPIO >= 1200) {
gpioLed(mesure[2][pMesure]);
compteurGPIO = 0;
}
pCommande++;
if (pCommande >= TAILLE_COMMANDE)
pCommande = 0;
vTaskDelayUntil( &xLastWakeTime, xFrequency );
}
close();
}
void main() {
timerInit();
sei();
pwmInit();
int16_t i = 0;
uint32_t lastDetentTimestamp;
pwmSet(0);
int8_t encoderPulses = 0;
while(1) {
encoderPulses += encoderRead();
if( encoderPulses > 3 || encoderPulses < -3 ) {
int8_t encoderStep = encoderPulses / 4;
if(millis - lastDetentTimestamp < 20) {
i+= encoderStep*10;
} else if(millis - lastDetentTimestamp < 30) {
i+= encoderStep*5;
} else if(millis - lastDetentTimestamp < 40) {
i+= encoderStep*2;
} else {
i+= encoderStep;
}
if(i > 187) {
i = 187;
}
if(i < 0) {
i = 0;
}
targetPwm = (uint8_t) pgm_read_byte(&(pwm[i]));
lastDetentTimestamp = millis;
//_delay_ms(1);
encoderPulses = 0;
}
// _delay_ms(5);
}
}
void Controller::motionController()
{
controlParam.controlValidity=0x01;
controllerMetaData.yawErrorPrev = controllerMetaData.yawErrorCurr;
controllerMetaData.yawErrorCurr = normailizeError(setPoint.yaw, sensorData.imuData.yaw);
controllerMetaData.yawErrorDiff = controllerMetaData.yawErrorCurr - controllerMetaData.yawErrorPrev;
controllerMetaData.yawErrorSum += controllerMetaData.yawErrorCurr;
controllerMetaData.pitchErrorPrev = controllerMetaData.pitchErrorCurr;
controllerMetaData.pitchErrorCurr = normailizeError(setPoint.pitch,sensorData.imuData.pitch);
controllerMetaData.pitchErrorDiff = controllerMetaData.pitchErrorCurr - controllerMetaData.pitchErrorPrev;
controllerMetaData.pitchErrorSum += controllerMetaData.pitchErrorCurr;
controllerMetaData.depthErrorPrev = controllerMetaData.depthErrorCurr;
controllerMetaData.depthErrorCurr = setPoint.posZ - pressureData.height;
controllerMetaData.depthErrorDiff = controllerMetaData.depthErrorCurr - controllerMetaData.depthErrorPrev;
controllerMetaData.depthErrorSum += controllerMetaData.depthErrorCurr;
if(controllerMetaData.yawErrorSum > 50)
{
controllerMetaData.yawErrorSum = 50;
}
if(controllerMetaData.yawErrorSum < -50)
{
controllerMetaData.yawErrorSum = -50;
}
if(controllerMetaData.pitchErrorSum > 50)
{
controllerMetaData.pitchErrorSum = 50;
}
if(controllerMetaData.pitchErrorSum < -50)
{
controllerMetaData.pitchErrorSum = -50;
}
pwmData.swayBack = DEFAULT_MOTOR_PWM_SWB;
pwmData.swayFront = DEFAULT_MOTOR_PWM_SWF;
pwmData.surgeLeft = DEFAULT_MOTOR_PWM_SUL;
pwmData.surgeRight = DEFAULT_MOTOR_PWM_SUR;
pwmData.heaveBack = DEFAULT_MOTOR_PWM_HB;
pwmData.heaveFront = DEFAULT_MOTOR_PWM_HF;
if (setPoint.velY>50.0) setPoint.velY=50.0;
else if (setPoint.velY<-50.0) setPoint.velY=-50.0;
if (setPoint.velX>50.0) setPoint.velX=50.0;
else if (setPoint.velX<-50.0) setPoint.velX=-50.0;
if ( controlParam.controlValidity & VALID_SWAY_CONTROL)
{
pwmData.swayBack += (int)controlParam.cSway*setPoint.velY;
pwmData.swayFront += (int)controlParam.cSway*setPoint.velY;
}
if ( controlParam.controlValidity & VALID_SURGE_CONTROL)
{
pwmData.swayBack += (int)controlParam.cSurge*setPoint.velX;
pwmData.swayFront += (int)controlParam.cSurge*setPoint.velX;
}
if ( controlParam.controlValidity & VALID_YAW_CONTROL)
{
if (setPoint.velY < 10.0)
{
pwmData.swayBack -=(int) controlParam.kpYaw*controllerMetaData.yawErrorCurr;
pwmData.swayBack -=(int) controlParam.kdYaw*controllerMetaData.yawErrorDiff;
pwmData.swayBack -=(int) controlParam.kiYaw*controllerMetaData.yawErrorSum;
pwmData.swayFront +=(int) controlParam.kiYaw*controllerMetaData.yawErrorSum;
pwmData.swayFront +=(int) controlParam.kdYaw*controllerMetaData.yawErrorDiff;
pwmData.swayFront +=(int) controlParam.kpYaw*controllerMetaData.yawErrorCurr;
}
else
{
int factor=1;
pwmData.surgeLeft +=(int) controlParam.kpYaw*controllerMetaData.yawErrorCurr;
pwmData.surgeLeft +=factor*(int) controlParam.kdYaw*controllerMetaData.yawErrorDiff;
pwmData.surgeLeft +=(int) controlParam.kiYaw*controllerMetaData.yawErrorSum;
pwmData.surgeRight -=(int) controlParam.kiYaw*controllerMetaData.yawErrorSum;
pwmData.surgeRight -=factor*(int) controlParam.kdYaw*controllerMetaData.yawErrorDiff;
pwmData.surgeRight -=(int) controlParam.kpYaw*controllerMetaData.yawErrorCurr;
}
}
if ( controlParam.controlValidity & VALID_DEPTH_CONTROL)
{
pwmData.heaveBack +=(int) controlParam.kpDepth*controllerMetaData.depthErrorCurr;
pwmData.heaveBack +=(int) controlParam.kdDepth*controllerMetaData.depthErrorDiff;
pwmData.heaveBack +=(int) controlParam.kiDepth*controllerMetaData.depthErrorSum;
pwmData.heaveFront +=(int) controlParam.kiDepth*controllerMetaData.depthErrorSum;
pwmData.heaveFront +=(int) controlParam.kdDepth*controllerMetaData.depthErrorDiff;
pwmData.heaveFront +=(int) controlParam.kpDepth*controllerMetaData.depthErrorCurr;
}
if ( controlParam.controlValidity & VALID_DEPTH_CONTROL)
{
pwmData.heaveBack +=(int) controlParam.kpDepth*controllerMetaData.depthErrorCurr;
pwmData.heaveBack +=(int) controlParam.kdDepth*controllerMetaData.depthErrorDiff;
pwmData.heaveBack +=(int) controlParam.kiDepth*controllerMetaData.depthErrorSum;
pwmData.heaveFront +=(int) controlParam.kiDepth*controllerMetaData.depthErrorSum;
pwmData.heaveFront +=(int) controlParam.kdDepth*controllerMetaData.depthErrorDiff;
pwmData.heaveFront +=(int) controlParam.kpDepth*controllerMetaData.depthErrorCurr;
}
if (controlParam.controlValidity & VALID_PITCH_CONTROL)
{
pwmData.heaveBack -=(int) controlParam.kpPitch*controllerMetaData.pitchErrorCurr;
pwmData.heaveBack -=(int) controlParam.kdPitch*controllerMetaData.pitchErrorDiff;
pwmData.heaveBack -=(int) controlParam.kiPitch*controllerMetaData.pitchErrorSum;
pwmData.heaveFront +=(int) controlParam.kiPitch*controllerMetaData.pitchErrorSum;
pwmData.heaveFront +=(int) controlParam.kdPitch*controllerMetaData.pitchErrorDiff;
pwmData.heaveFront +=(int) controlParam.kpPitch*controllerMetaData.pitchErrorCurr;
}
cap(pwmData.swayBack);
cap(pwmData.swayFront);
cap(pwmData.surgeLeft);
cap(pwmData.surgeRight);
cap(pwmData.heaveBack);
cap(pwmData.heaveFront);
pwmSet(pwmData);
}
/**
* @fn int main( void );
* @brief Point d'entrée du programme
* @return
*/
int main(void){
char ordre[50];
char cons[10];
int ref=18;
int pwm=0;
int fil=0;
char BUF_RX[50];
char BUF_TX[1000];
int aux1;
char *data_IL, *data_VO, *data_VI;
//Optimisation du fonctionnement du CPU
SYSTEMConfig(SYS_FREQ, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);
init();
intConfig();
uartPutString(" ****** * * ******* ****** *** ****** ** ****** *** ***\r\n");
uartPutString("* **** * * *** *** * *** * * * * **** * * * **** * * * *\r\n");
uartPutString("* * * * * * * *** * * * * * * * ** * * * * * * * *\r\n");
uartPutString("* * * * * * * ** * * *** * * * * **** * * *** * ** ** *\r\n");
uartPutString("* * * * * * * * * * * * * * * * * * * * * * * * * *\r\n");
uartPutString("* **** * * * * * * * * * * * **** * * * * * **** * * * *\r\n");
uartPutString(" ****** * * * * * * * * *** ****** * * * * ****** * * * *\r\n");
uartPutString("citroen corp. with Ixchel Intelligent Systems patnership\r\n");
strcpy(BUF_TX,"boost converter module interface. Enter help to know the supported commands\r\nboost:~# \0");
uartPutString(BUF_TX);
while (1) {;
if (flagTraitement) {
flagTraitement = 0;
// Acquisition
ads7885Pic32Read( CHN_SPI, data_IL, data_VO, data_VI );
mesure[0][pMesure]=atoi(data_IL);
mesure[1][pMesure]=atoi(data_VO);
mesure[2][pMesure]=atoi(data_VI);
pMesure++;
if (pMesure >= TAILLE_MESURE)
pMesure = 0;
gpioLed();
// Calcul
commande[pCommande] = fTransfert(consigne, mesure[0], commande);
//
// // Commande
// pwmSet(commande[pCommande]);
// pCommande++;
// if (pCommande >= TAILLE_COMMANDE)
// pCommande = 0;*/
if (flagAuto == 0){
pwmSet(pwm);
}
}
if (flagReception) {
uartPutChar(aux);
if(aux=='\b'){
if(i>0) i--;
}
else if(aux!='\r'){
BUF_RX[i]=aux;
i++;
}
else
{
BUF_RX[i]='\0';
uartPutChar('\r');
uartPutChar('\n');
i=0;
// On reçoit l'ordre
while(BUF_RX[i]!=' '){
ordre[i]=BUF_RX[i];
i++;
}
ordre[i]='\0';
i++;
int j=0;
while(BUF_RX[i]!='\0'){
cons[j]=BUF_RX[i];
i++;
j++;
}
cons[j]='\0';
i=0;
if(strcmp(ordre, "auto")==0){
strcpy(BUF_TX,"automatic mode is running\n\rchange voltage reference value with ref command\n\r");
flagAuto = 1;
ref=18;
}
else if(strcmp(ordre, "manual")==0){
strcpy(BUF_TX,"pwm mode is running\n\rchange duty cycle value with pwm command\n\r");
flagAuto = 0;
pwm=0;
}
else if(strcmp(ordre, "ref")==0){
sscanf(cons,"%i",&aux1);
if((aux1!=18)&&(aux1!=24)&&(aux1!=30)&&(aux1!=36)){
strcpy(BUF_TX,"error: failed value\n\r");
}
else {
ref=aux1;
sprintf(BUF_TX,"output voltage value is updated to %d\n\r",ref);
}
}
else if(strcmp(ordre, "pwm")==0){
sscanf(cons,"%i",&aux1);
if((aux1>=0)&&(aux1<=100)&&(strcmp(cons, "")!=0)){
sprintf(BUF_TX,"the duty cycle value is updated to %d%\n\r",aux1);
pwm= (aux1*T3_TICK)/100;
}
else {
strcpy(BUF_TX,"error: failed value\n\r");
}
}
else if(strcmp(ordre, "means")==0);//{
// break;
// }
else if(strcmp(ordre, "can")==0){
uartPutString("can controller configuration :\n\r");
sprintf(BUF_TX,"bus speed 250000 bps\n\r");
uartPutString(BUF_TX);
sprintf(BUF_TX,"data length 8 bytes\n\r");
uartPutString(BUF_TX);
sprintf(BUF_TX,"filter 0x%03X\n\r",fil);
uartPutString(BUF_TX);
strcpy(BUF_TX,"filter mask 0xFFF\n\r");
}
else if(strcmp(ordre, "filter")==0){
if(strlen(cons)==3){
sscanf(cons,"%X",&fil);
sprintf(BUF_TX,"can filter value is updated to 0x%03X\n\r",fil);
}
else {
sprintf(BUF_TX,"error: failed value size= %d\n\r",strlen(cons));
}
}
else if(strcmp(ordre, "help")==0){
uartPutString("auto start automatic mode and stop manual mode\r\n");
uartPutString(" update voltage reference value with ref command\r\n");
uartPutString("manual start manual mode and stop automatic mode\r\n");
uartPutString(" update duty cycle value with pwm command\r\n");
uartPutString("ref update output voltage reference value\r\n");
uartPutString("pwm update duty cycle value\r\n");
uartPutString("means print currents analog values\r\n");
uartPutString("can print can controller configuration\r\n");
uartPutString("filter update can filter\r\n");
sprintf(BUF_TX,"help print supported commands\r\n");
}
else if(strcmp(ordre, "")==0){
strcpy(BUF_TX,"\r\n");
}
else{
strcpy(BUF_TX,"error: enter help to know the supported commands\n\r");
}
uartPutString(BUF_TX);
strcpy(BUF_TX,"boost:~# \0");
uartPutString(BUF_TX);
}
flagReception = 0;
if (consigne < 0)
consigne = 0;
else if (consigne > 262143)
consigne = 262143;
}
}
close();
}
/**
* Main program.
*/
int main()
{
uchar i, errorCode;
uint8_t oldRunning = 0, running;
/* Disable the watchdog */
MCUSR = 0;
wdt_disable();
/* Read the serial number from flash */
readSerial();
/* Read the flash checksum from EEPROM */
readFlashChecksum();
/* Initialize USB and enable global interrupts */
initUSB();
/* First initialize the PWM so all LEDs are off. */
pwmInit(prescalingIndex);
pwmSet(CHANNEL0, 0, 0);
pwmSet(CHANNEL1, 0, 0);
pwmSet(CHANNEL2, 0, 0);
pwmSet(CHANNEL3, 0, 0);
pwmSet(CHANNEL4, 0, 0);
pwmSet(CHANNEL5, 0, 0);
/* Initialize the blinks memory from eeprom */
eeprom_busy_wait();
eeprom_read_block(memory, (uint8_t*) 1, BLINKS_MEM_SIZE);
/* Make sure bits 7 is cleared and bit 6 is set */
memory[0] &= 0x7f;
memory[0] |= 0x40;
/* Initialize blinks */
blinksScript.data = BLINKS_SCRIPT(memory);
blinksScript.size = BLINKS_SCRIPT_SIZE;
errorCode = 1;
/* Infinite program loop */
i = 0;
uint16_t shutdown = 0;
while ((memory[0] & 0x40) || (--shutdown))
{
/* Re-init PWM if needed */
if (prescalingIndex != getPrescalingIndex())
{
prescalingIndex = getPrescalingIndex();
pwmInit(prescalingIndex);
}
/* Process USB events */
usbPoll();
/* Execute special commands */
switch (command)
{
case CMD_SAVE:
eeprom_busy_wait();
eeprom_write_block(memory, (uint8_t*) 1, BLINKS_MEM_SIZE);
break;
}
command = 0;
/* Reset script when running flag has changed or an error occurred */
running = BLINKS_FLAGS(memory) & BLINKS_FLAG_RUNNING;
if (running != oldRunning || errorCode)
{
errorCode = blinksReset(blinksScript);
oldRunning = running;
}
/* Execute next step in program when program is running an no error
occurred. */
if (running && !errorCode)
errorCode = blinksStep(blinksScript);
/* Apply channel values */
if (!errorCode)
{
unsigned char outputs = blinksCountOutputs(blinksScript);
unsigned char invert = isInverted();
if (outputs > 0) pwmSet(CHANNEL0, blinksGetOutput(blinksScript, 0), invert);
if (outputs > 1) pwmSet(CHANNEL1, blinksGetOutput(blinksScript, 1), invert);
if (outputs > 2) pwmSet(CHANNEL2, blinksGetOutput(blinksScript, 2), invert);
if (outputs > 3) pwmSet(CHANNEL3, blinksGetOutput(blinksScript, 3), invert);
if (outputs > 4) pwmSet(CHANNEL4, blinksGetOutput(blinksScript, 4), invert);
if (outputs > 5) pwmSet(CHANNEL5, blinksGetOutput(blinksScript, 5), invert);
}
/* When this loop has been executed for 100 times then
mark the flash as OK */
if ((flashChecksum != FLASH_CHECKSUM) && (i < 100))
{
i++;
if (i == 100)
{
flashChecksum = FLASH_CHECKSUM;
writeFlashChecksum();
}
}
}
enterBootloader();
return 0;
}
/**
* Main program.
*/
int main()
{
uchar i;
/* Disable the watchdog */
MCUSR = 0;
wdt_disable();
/* Read the flash checksum from EEPROM */
readFlashChecksum();
/* Initialize USB and enable global interrupts */
initUSB();
/* First initialize the PWM so all LEDs are off. */
pwmInit(0);
pwmSet(CHANNEL0, 0, 0);
pwmSet(CHANNEL1, 0, 0);
pwmSet(CHANNEL2, 255, 0);
pwmSet(CHANNEL3, 0, 0);
pwmSet(CHANNEL4, 0, 0);
pwmSet(CHANNEL5, 0, 0);
/* Initialize the blinks memory from eeprom */
eeprom_busy_wait();
eeprom_read_block(memory, (uint8_t*) 1, MEM_SIZE);
/* Make sure bits 7 is cleared and bit 6 is set */
memory[0] &= 0x7f;
memory[0] |= 0x40;
/* Infinite program loop */
i = 0;
while (memory[0] & 0x40)
{
restart = 50;
while (restart)
{
/* Process USB events */
usbPoll();
int len = buffer_write_index - buffer_read_index;
if (len > 0 && usbInterruptIsReady())
{
if (len > 8) len = 8;
usbSetInterrupt(&buffer[buffer_read_index], len);
buffer_read_index += len;
to_read -= len;
if (!to_read)
{
buffer_read_index = 0;
buffer_write_index = 0;
}
}
if (restart < 50) restart--;
/* When this loop has been executed for 100 times then
mark the flash as OK */
if ((flashChecksum != FLASH_CHECKSUM) && (i < 100))
{
i++;
if (i == 100)
{
flashChecksum = FLASH_CHECKSUM;
writeFlashChecksum();
}
}
}
reenumerate(500);
}
enterBootloader();
return 0;
}
