brenodee wrote on Thursday, March 13, 2008:
Hi all, I’m posting a huge code that I’m using to control an cc motor and adjust the positions of wheels in a car. The bug occurs when I start 3 threads to run. It works fine on pairs (ex. control_velocity and other) but it completely stuck when a got 3 tasks created.
I’m not sure if it’s a memory issue or processor capacity problem.
I’m using Atmega128 my configTOTAL_HEAP_SIZE is 4*1024.
To sum up, I have three tasks, one to run the velocity control law, another one to adjust the direction of the wheels and another one to catch the value of AD converter. I used some mechanism to synchronization with tasks and interruptions
I hope find someone patient to take a look.
// * Definitions
/* Includes */
#include <stdio.h>
#include <avr/io.h>
#include <stdlib.h>
#include <avr/interrupt.h>
#include <math.h>
#ifdef GCC_MEGA_AVR
/* EEPROM routines used only with the WinAVR compiler. */
#include <avr/eeprom.h>
#endif
/* Scheduler include files. */
#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"
#include <avr/interrupt.h>
#include <avr/sleep.h>
/* Demo file headers. */
#include "serial.h"
float get_velocity(void);
/* Priority definitions for most of the tasks in the demo application. Some
tasks just use the idle priority. */
/* Baud rate used by the serial port*/
#define mainCOM_TEST_BAUD_RATE ( ( unsigned portLONG ) 38400 )
#define TwoTasks
//#define OneTask
#define VelocidadeCFiltro
#define filtroRef
//#define ContagemTicks
#define adc
//void transmit_value (float var);
//void transmit_value (float var,float ref);
float get_velocity( void );
void adjust_PH_vel(char direction);
void adjust_DutyPWM_vel(float duty);
void vApplicationIdleHook( void );
void control_velocity( void );
#ifdef TwoTasks
static void adjust_DirPos(void);
#endif
#ifdef OneTask
void adjust_DirPos(float pos);
#endif
#ifdef adc
static void getDistanciaObstaculo();
#endif
/*
* Global variables
*/
int pulse;
float ref_pos = 6.7;
float ref_vel = 80;
int iterations = 0;
unsigned char ccc=0;
char ini[] = "Programa Iniciado!!!\n\r";
unsigned char buffer[5];
#ifdef adc
xSemaphoreHandle semaforoADC = NULL;
#endif
int main( void )
{
DDRA = 0xFF;
xSerialPortInitMinimal( mainCOM_TEST_BAUD_RATE, 1 );
/********************** CONFIG INT*************************/
DDRB = 0xFF;
DDRC = 0xFF;
DDRE |= 0b00001000;
MCUCR = 0x03;
/*************************PWM’s*************************/
//Configuracao da saida de PWM A, B e C do Timer1(PB5) e 3(PE3)
//Configs Velo - PWM0, POS - PWM1
TCCR0 = 0b01101010;
TCCR3A = 0b10000010;
TCCR3B = 0b00010010;
//Configs Velo - PWM3, Pos - PWM0
// TCCR0 = 0b01100110;
// TCCR3A = 0b10000010;
// TCCR3B = 0b00011010;
/*********************Set Freq PWM1 to 2khz and PWM3 6khz************/
OCR0 = 0x00;
//configs para velocidade a 4 khz
ICR3 = 0x2710;
//configs para velocidade a 6khz
// ICR3 = 0xA5; //==165 -> 166 cont’s
// ******************ADC CONFIG.*********************************/
ADMUX = 0x00;
// //Habilita ADC
ADCSRA = 0x88;
// ******************
/*****************Interruptions*************************/
SREG = 0x80;
EICRA = 0x03; // 3 - 0
EICRB = 0x00; // 7 - 4
EIMSK = 0x01;
/****************************** END of conf.*************************/
string_transmit(ini);
#ifdef adc
vSemaphoreCreateBinary( semaforoADC );
#endif
ref_vel = 80;
adjust_PH_vel(0);
xTaskCreate ( control_velocity, "Vel", configMINIMAL_STACK_SIZE, NULL, 2, NULL );
#ifdef TwoTasks
xTaskCreate (adjust_DirPos, "Dir", 30, NULL, 2 , NULL);
#endif
#ifdef adc
// xTaskCreate (getDistanciaObstaculo,"obs", 30, NULL,2,NULL);
#endif
vTaskStartScheduler();
return 0;
}
void control_velocity( void )
{
char firstTime = 1;
float control[2],control2;
float error[2],new_velocity[4],final_velocity[4];
#ifdef filtroRef
float referencia[2];
#endif
#ifdef ContagemTicks
char tick;
#endif
while(1)
{
/******************************* Initializations********************************/
if(firstTime ==1)
{
new_velocity[0]=get_velocity();
error[0]=ref_vel-new_velocity[0];
control[0]= 0;
control[1] = 0;
error[1]=error[0];
new_velocity[1]=new_velocity[0];
new_velocity[2]=new_velocity[0];
new_velocity[3]=new_velocity[0];
#ifdef VelocidadeCFiltro
final_velocity[1]=new_velocity[0];
final_velocity[2]=new_velocity[0];
final_velocity[3]=new_velocity[0];
#endif
#ifdef filtroRef
referencia[0] = 0;
referencia[1] = 0;
#endif
firstTime = 0;
}
/****************************** Control Law*******************************/
new_velocity[3]=new_velocity[2];
new_velocity[2]=new_velocity[1];
new_velocity[1]=new_velocity[0];
//GET Velocity
new_velocity[0]=get_velocity();
#ifdef ContagemTicks
// tick = xTaskGetTickCount ();
tick = TCNT1;
#endif
#ifdef VelocidadeCFiltro
final_velocity[3]=final_velocity[2];
final_velocity[2]=final_velocity[1];
final_velocity[1]=final_velocity[0];
//Filtro de terceira ordem na leitura
// final_velocity[0]=0.332*new_velocity[0]+0.995*new_velocity[1]+0.995*new_velocity[2]+0.332*new_velocity[3]-1.15*final_velocity[1]-0.44*final_velocity[2]-0.057*final_velocity[3];
final_velocity[0]=0.4*new_velocity[0]+0.6*final_velocity[1];
#endif
//Calculo do Erro
error[1] = error[0];
#ifdef VelocidadeCFiltro
#ifndef filtroRef
error[0]=ref_vel-final_velocity[0];
#endif
#ifdef filtroRef
referencia[1] = referencia[0];
referencia[0] = 0.2*ref_vel+0.8*referencia[1];
error[0]=referencia[0]-final_velocity[0];
#endif
#endif
#ifndef VelocidadeCFiltro
error[0]=ref_vel-new_velocity[0];
#endif
control[1] = control[0];
//Calculo do Controle (PI discreto)
control[0]=(2.25*error[0])-(1.95*error[1])+control[1];
//Aproximacao da Look-up Table (tensao por velocidade)
control2=0.015*control[0]+5.013;
control2=control2*13.88;
if(abs(control2)>100)
adjust_DutyPWM_vel(100);
else
adjust_DutyPWM_vel(abs(control2));
#ifdef ContagemTicks
// tick = xTaskGetTickCount() -tick;
tick = TCNT1 - tick;
// transmit_value((float)tick);
itoa((int)tick,buffer,10);
string_transmit(buffer);
#endif
PORTA = ~0x01;
}
}
#ifdef TwoTasks
static void adjust_DirPos(void)
{
while(1)
{
OCR3A = 100*ref_pos;
vTaskDelay(50);
}
}
#endif
#ifdef OneTask
void adjust_DirPos (float pos)
{
if(pos<6.7)
{OCR3A = 670;}
else
{
if(pos>7.9)
{OCR3A = 790;}
else
{OCR3A = 100*pos;}
}
}
#endif
float get_velocity()
{
pulse =0;
EIMSK = 0x01;
vTaskDelay(( portTickType ) 300/ portTICK_RATE_MS);
EIMSK = 0x00;
return (pulse*3.78);
}
void adjust_DutyPWM_vel(float duty)
{
//config to velocity at 4khz
OCR0 = 2.55*duty;
//config to velocity at 6khz
// OCR3A = 1.65*duty;
}
void adjust_PH_vel(char direction)
{
//1 - backward
//0 - forward
if(direction==1)
PORTB = (PORTB&(0b11111100))|(0b10);
else
PORTB = (PORTB&(0b11111100))|(0b01);
}
#ifdef adc
static void getDistanciaObstaculo()
{
static int adcValue = 0;
static float distancia = 0;
while(1){
ADCSRA = 1<<6|ADCSRA;
if( xSemaphoreTake( semaforoADC, 10 ) == pdTRUE ){
adcValue = ADCL;
adcValue |= ADCH<<8;
distancia = (adcValue/204.8);
if(distancia <0.7){
PORTA = ~0x01;
}else{
PORTA = ~0x80;
}
}
}
}
#endif
/***********Tratadores de Interrupcao************************/
#ifdef adc
ISR(ADC_vect)
{
xSemaphoreGiveFromISR( semaforoADC, pdFALSE );
}
#endif
ISR(INT0_vect)
{
pulse++;
}
ISR(USART0_RX_vect)
{
ccc = UDR0;
switch(ccc)
{
case ‘a’:
ref_vel = 100;
break;
case ‘b’:
ref_vel = 70;
break;
case ‘c’:
ref_vel = 120;
break;
case ‘A’:
#ifdef OneTask
adjust_DirPos(7);
#endif
#ifdef TwoTasks
ref_pos = 7;
// xTaskResumeFromISR(dirHandle);
#endif
break;
case ‘B’:
#ifdef OneTask
adjust_DirPos(6.7);
#endif
#ifdef TwoTasks
ref_pos = 6.7;
// xTaskResumeFromISR(dirHandle);
#endif
break;
case ‘C’:
#ifdef OneTask
adjust_DirPos(7.9);
#endif
#ifdef TwoTasks
ref_pos = 7.9;
// xTaskResumeFromISR(dirHandle);
#endif
break;
default:
break;
}
}
/*************** USART***********************************/
void string_transmit(unsigned char str[])
{
int counter = 0;
while(str[counter]!=’\0’)
{
/* Wait for empty transmit buffer */
while ( !(UCSR0A & (1<<UDRE0)) )
;
/* Start transmittion */
UDR0 = str[counter];
counter++;
}
}
/******************TRANSMISSÃO DE DADOS**************
void transmit_value (float var,float ref)
{
static char buffer[5];
// itoa((int)ref_vel,buffer,10);
itoa((int)ref,buffer,10);
string_transmit(buffer);
string_transmit("\t");
itoa((int)var,buffer,10);
string_transmit(buffer);
string_transmit("\n\r");
}
/******************IDLE TASK************************************/
void vApplicationIdleHook( void )
{
vCoRoutineSchedule();
}