 |
 |
| View previous topic :: View next topic |
| Author |
Message |
mikesbikes
Joined: 02 Jul 2009
Posts: 3
|
| Project CCS/PIC/VCP/Simulink |
 Posted: Thu Jul 02, 2009 5:43 pm |
 |
|
The project I am going to describe to you is the following:
WMR with two DC motors, two encoders, a PIC 16F876A with bootloader and a pc on top with simulink. I used a VCP to interface the PIC with the pc´s usb port.
I am sending the velocity references from simulink to pic, and receiving real velocities in the meanwhile. The CCS code used to program the pic follows.
The problems are, the rs232 blocks often, and if I don´t add some repetitive code it simply doesn´t work. Suggestions would be very appreciated. Thank you.
OS Windows XP - SP3; Matlab R2008b; CCS 4.084.
| Code: | #include <16F876A.h>
#device ICD=TRUE
#device adc=8
#use fast_io(b) //_inputs=PIN_b4, PIN_b5, PIN_b6, PIN_b7)
#use delay(clock=20000000)
#fuses NOWDT,HS, NOPUT, NOPROTECT, DEBUG, BROWNOUT, NOLVP, NOCPD, NOWRT
#use rs232(baud=9600,parity=N,xmit=PIN_C6,rcv=PIN_C7,bits=8)
// Global vars 115200
#byte Portb = 6
// CONTADORES
signed int32 counter_D=0;
signed int32 counter_E=0;
// Real velocity of the wheels
signed int32 vel_D=0;
signed int32 vel_E=0;
// Simulink velocity orders
static signed int32 vel_in_D =0, erro_ant_D = 0, erro_ant_E = 0;
static signed int32 vel_in_E=0;
// velocity error
signed int32 erro_D, erro_E;
// PWM out
signed int32 pwm_out_D,pwm_out_E;
signed int32 value, valueanterior, valueRoda2, valueanteriorRoda2, valor;
int1 TIMERFLAG=0;
signed int32 Gc_D = 0,Gc_E = 0;
signed int32 Kp_D = 10, Ki_D = 50, Kp_E= 10, Ki_E= 50;
signed int32 yi_D = 0, yi_ant_D = 0, yi_E = 0, yi_ant_E = 0;
#include <math.h>
// Buffer definitions
#define MAX_MSG_SIZE 20
char CURRENT_COMMAND[MAX_MSG_SIZE];
unsigned int CURRENT_COMMAND_LEN = 0;
#define STATE_STAND_BY 's'
#define STATE_COMMAND 'c'
char CURRENT_STATE = STATE_STAND_BY;
// serial port write to simulink
void write_serial(){
printf("#%c%c%c%c\r\n",vel_D, vel_E, vel_in_D, vel_in_E);
}
// PWM out
void write_pwm(){
set_pwm1_duty(pwm_out_D);
set_pwm2_duty(pwm_out_E);
}
void doCommand(){
// if order from simulink is ok, update velocity references
if(CURRENT_COMMAND_LEN==5 && CURRENT_COMMAND[0]=='D' && CURRENT_COMMAND[2]=='F'){
write_serial();
vel_in_D=CURRENT_COMMAND[1]; // this is where I have to repeat
vel_in_D=CURRENT_COMMAND[1]; // the commands in order to put the program
vel_in_D=CURRENT_COMMAND[1]; // running
vel_in_D=CURRENT_COMMAND[1];
vel_in_E=CURRENT_COMMAND[3];
vel_in_E=CURRENT_COMMAND[3];
vel_in_E=CURRENT_COMMAND[3];
vel_in_E=CURRENT_COMMAND[3];
}
// Buffer reset
memset(CURRENT_COMMAND, 0, MAX_MSG_SIZE);
CURRENT_COMMAND_LEN = 0;
CURRENT_STATE = STATE_STAND_BY;
}
// Serial interruption
#int_rda
void serial_isr(){
char ch;
ch=getc();
// if buffer is full, then reset buffer
if(CURRENT_COMMAND_LEN == MAX_MSG_SIZE){
memset(CURRENT_COMMAND, 0, MAX_MSG_SIZE);
CURRENT_COMMAND_LEN = 0;
}
// if terminator is not received, save info in buffer
if(ch != '\n'){
CURRENT_COMMAND[CURRENT_COMMAND_LEN] = ch;
CURRENT_COMMAND_LEN++;
}
// otherwise check if there´s another terminator (there are two terminators)
else {
if(CURRENT_COMMAND_LEN >=1)
// if there are 2 terminators, then change STATE_COMMAND
if(CURRENT_COMMAND[CURRENT_COMMAND_LEN-1]=='\r')
CURRENT_STATE = STATE_COMMAND;
else{
CURRENT_COMMAND[CURRENT_COMMAND_LEN] = ch;
CURRENT_COMMAND_LEN++;
}
}
}
// Ts do calculo da vel. ang. das rodas
// Ts = 255/(20e6/4/256) = 0.0131 ms
// Ts(SIMULINK) = 3*0.0131 = 0.0393 = 0.04
// RTCC interrption - real velocity calculus
#INT_RTCC
void clock_isr(){
//vel = counter * 2pi/400/Ts(PIC) = counter*1.1991
vel_D = counter_D*1.2;
vel_E = counter_E*1.2;
TIMERFLAG=1;
}
// portB interruption
#int_RB
void RB_isr(void){
// read encoder A
valor=input_b()&0x80;
if (valor != valueanterior) ++counter_D;
valueanterior=valor;
// read encoder B
valueRoda2=input_b()&0x20;
if (valueRoda2 != valueanteriorRoda2) ++counter_E;
valueanteriorRoda2=valueRoda2;
}
void main(){
setup_adc_ports (AN0_AN1_AN3);
setup_adc (ADC_CLOCK_INTERNAL);
setup_timer_2 (T2_DIV_BY_4, 255, 1);//76Hz ou 38Hz
setup_ccp1 (CCP_PWM);
setup_ccp2 (CCP_PWM);
SET_TRIS_B (0x0F);
SET_TRIS_C (0xF0);
output_bit( PIN_C3, 0);
set_pwm2_duty (0); //Inicializa a 0 os motores 1 e 2
output_bit( PIN_C0, 0);
set_pwm1_duty (0);
set_tris_b (0xF0);
port_b_pullups(TRUE);
delay_us(10);
setup_timer_0( RTCC_INTERNAL | RTCC_DIV_256 );
set_timer0(0);
enable_interrupts(INT_RTCC);
enable_interrupts(INT_RB); // turn on port b interrupt on change
enable_interrupts(int_rda);
enable_interrupts(GLOBAL);
setup_timer_2 (T2_DIV_BY_4, 255, 1);
printf("ta vivo");
WHILE (TRUE){
// change state
switch(CURRENT_STATE){
case STATE_STAND_BY:
break;
case STATE_COMMAND:{
doCommand();
break;
}
}
if(TIMERFLAG==1){
disable_interrupts(INT_RB);
disable_interrupts(INT_RTCC);
// control for right motor
erro_D = (vel_in_D-vel_D);
yi_D = Ki_D*0.0131*erro_ant_D + yi_ant_D;
Gc_D = Kp_D*erro_D + (yi_D + yi_ant_D);
if(Gc_D >1024) pwm_out_D = 1024;
else if (Gc_D<0) pwm_out_D=0;
else pwm_out_D = Gc_D;
erro_ant_D = erro_D;
yi_ant_D = yi_D;
counter_D=0;
// control for left motor
erro_E = (vel_in_E-vel_E);
yi_E = Ki_E*0.0131*erro_ant_E + yi_ant_E;
Gc_E = Kp_E*erro_E + (yi_E+ yi_ant_E);
if(Gc_E > 1024) pwm_out_E = 1024;
else if (Gc_E < 0) pwm_out_E=0;
else pwm_out_E = Gc_E;
erro_ant_E = erro_E;
yi_ant_E = yi_E;
counter_E=0;
write_pwm(); // PWM out
clear_interrupt(INT_RB);
clear_interrupt(INT_RTCC);
set_timer0(0);
enable_interrupts(INT_RTCC);
enable_interrupts(INT_RB);
TIMERFLAG=0;
}
}
}
|
|
|
 |
PCM programmer
Joined: 06 Sep 2003
Posts: 21708
|
|
| Posted: Thu Jul 02, 2009 7:49 pm |
|
|
| Quote: | | The problems are, the rs232 blocks often |
To see the problem, turn on compiler warnings in the Project Build
Options menu:
| Quote: | >>> Warning 216 "pcm_test.c" Line 213(0,1): Interrupts disabled during call to prevent re-entrancy: (@SDTOF)
>>> Warning 216 "pcm_test.c" Line 213(0,1): Interrupts disabled during call to prevent re-entrancy: (@MULFF)
>>> Warning 216 "pcm_test.c" Line 213(0,1): Interrupts disabled during call to prevent re-entrancy: (@FTOSD) |
This means you calling CCS math library routines both inside an
interrupt service routine and outside it. The floating point math code
shown in bold below is causing the problem:
| Quote: |
#INT_RTCC
void clock_isr(){
//vel = counter * 2pi/400/Ts(PIC) = counter*1.1991
vel_D = counter_D*1.2;
vel_E = counter_E*1.2;
TIMERFLAG=1;
}
|
The math routines are not re-entrant, so the compiler must disable
interrupts for the floating point math routines when it's called outside of
the isr. This is probably what's causing your #int_rda interrupt to block.
Floating point math routines can take a long time to execute.
To fix it, you can create a 2nd instance of the CCS math library code by
using #org default. Example:
| Quote: |
#org 0x1000, 0x11FF DEFAULT
#INT_RTCC
void clock_isr(){
vel_D = (counter_D * 1.2);
vel_E = (counter_E * 1.2);
TIMERFLAG=1;
}
#org DEFAULT
void main(){
setup_adc_ports (AN0_AN1_AN3);
setup_adc (ADC_CLOCK_INTERNAL);
|
To minimize the amount of library code that's duplicated when you
use #org default, it might be better to use integer math inside the isr,
instead of floating point:
| Quote: | #org 0x1000, 0x11FF DEFAULT
#INT_RTCC
void clock_isr(){
vel_D = (counter_D * 6) / 5;
vel_E = (counter_E * 6) / 5;
TIMERFLAG=1;
} |
This assumes that the counter_D and counter_E values are small enough
that they won't overflow a signed int32 if they are multiplied by 6. |
|
|
mikesbikes
Joined: 02 Jul 2009
Posts: 3
|
|
| Posted: Fri Jul 10, 2009 8:42 pm |
|
|
I did what you suggested and it still blocks. What I did next was, to comment the following lines,
| Quote: |
clear_interrupt(INT_RB);
clear_interrupt(INT_RTCC);
|
Now it works OK!
Nevertheless, I read the help about the #org command, but I couldn't understand the process of determining the memory block to allocate to a specific function. That must be interconnected with the pic datasheet. Can you give me some advice/orientation. Thank you. |
|
|
mikesbikes
Joined: 02 Jul 2009
Posts: 3
|
| Moving to HI-TECH |
| Posted: Fri Jul 24, 2009 7:09 am |
|
|
There are too many bugs in CCS, at least for the microcontroller used.
So now I'm reading the pic datasheet to reach the same objective with Microchip's compiler HI-TECH.
_________________
This project is at http://www.dissertacaodomike.hostoi.com/ |
|
|
Ttelmah
Guest
|
|
| Posted: Fri Jul 24, 2009 7:55 am |
|
|
Before you do so, stop, and re-think your approach.
Seriously, you have an RTCC interrupt that will have to take two signed 32 bit values, convert these to float values, and multiply by 1.2. Total time, probably about a mSec. For servo algorithms, _you_ need to understand the time limitations of the chip, and deal with these. A classic 'route' for example, would be to ue int16 values instead, and multiply by 307, using 'mul', then take the top 16 bits of the result. Total time probably under 100uSec (307/256 = *1.1992).
If you look at servo code written by Microchip, in their own C, they don't just use the internal functions, they do a lot of 'cheating' like this, to get performance. They have spent the time to work out code efficient ways to do things first...
You are going to have to do this, whatever compiler you use.
Also, general comment, add the keyword 'errors' to your RS232 definition. You should _always_ have this with the hardware RS232, unless _you_ are handling hardware overruns yourself.
Best Wishes |
|
|
|
|
You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot vote in polls in this forum
|
Powered by phpBB © 2001, 2005 phpBB Group
|
FAQ 
Search
Register
Profile 
Log in to check your private messages 
Log in
