Trasovani pro Servo
Napsal: 05 dub 2009, 23:03
mam takto napsany program na trasovani serva pomoci PWM ale mam problem s ukladanim dat do pole a nacitanim s pole. Data chci ukladat v Uhlech 0-180 a cas pro delay v kazde pozici v ms. Po nacteni se uhli prepoctou na delky pulzu PWM a serva se natoci do pozic a tam pockaji podle nastavene konstanty pro delay. Poradi mi nekdo kde je chyba?
Kód: Vybrat vše
//************************************************************************
#include "main.h"
#include <avr/interrupt.h>
#include <avr/io.h>
#include <avr/delay.h>
#include "lcd_led_matrix__lcd.h"
#include "delay_1ms_u16.h"
//************************************************************************
volatile u08 key_counter = 0;
volatile u08 key = 0;
volatile u08 old_key = 0;
//konstanty
// ************************************************************************************************************
SIGNAL(SIG_OUTPUT_COMPARE0) // signal handler for Timer/Counter-0 compare match
{
u08 tmp;
if (!lcd_port_locked()) { // test if no LCD write operation is in progress
LCD_DATA_DDR = 0x0F;
LCD_DATA_PORT = (~(1 << key_counter) & 0x0F);
_delay_loop_2(10e-6 * F_CPU); // 10us
tmp = ~LCD_DATA_PIN;
LCD_DATA_DDR = 0xFF;
if ((tmp & 0xF0) != 0) {
if (tmp != old_key) {
old_key = tmp;
key = tmp;
key_counter = 0;
}
}
else {
key_counter++;
if (key_counter >= 4) {
key_counter = 0;
old_key = 0;
}
}
}
}
// ************************************************************************************************************
void init(void)
{
// disable JTAG On-chip debug system
MCUCSR = 0x80;
MCUCSR = 0x80;
// Timer/counter-0 .. CTC mode, prescaler=64, top=230 => 1ms
TIMSK = (1<<OCIE0);
TCNT0 = 0;
OCR0 = 230;
TCCR0 = (1<<WGM01) | (1<<CS02) | (1<<CS00);
sei();
}
//************************************************************************
int main(void)
{
//const char autostep=10;
unsigned int elv=1;
unsigned int prepocetH=90;
unsigned int prepocetL=90;
unsigned int cas_konst=0;
char info1[20];
char info4[20];
char info2[20];
char info3[20];
unsigned int multi_array[10][3]; //nastavi pole 10x3
int i=0;
//int j;
//uvitaci text
const char str[20] = "VUT BRNO";
const char Txt[20] = "Bc.Lukas Konecny";
DDRE = 0x18; //povoleni portu PE3 a PE4 jako vystup PWM (OC3A a OC3B)
ICR3=(36864)/2;
TCCR3A=0xA0;
TCCR3B=0x12;
TCNT3H=0x00;
TCNT3L=0x00;
OCR3A=(2764.8)/2;
OCR3B=(2764.8)/2;
u08 line = 0;
init();
delay_1ms_u16(20);
init_lcd();
cursor_disp_ctrl(6);
// display startup message
clear_disp();
put_string_xy(4, 0, str);
put_string_xy(0, 1, Txt);
_delay_ms(1500);
clear_disp();
sprintf(info4, "Hi: %d°",prepocetH);
sprintf(info2, "Lo: %d°",prepocetL);
sprintf(info3, "t: %dsec",cas_konst);
put_string_xy(0, 0, info4);
cursor_disp_ctrl(0x04);
put_string_xy(8, 0, info2);
put_string_xy(0, 1, info3);
//obsluha klavesnice a volba hodnot
key = 0;
while(i<=9){
// display button name
switch (key) {
case 0x11 : {prepocetH+=1;key = 0;beep(200, 10);break;} // 1
case 0x12 : {prepocetH+=10;key = 0;beep(200, 10);break;} // 2
case 0x14 : {prepocetH+=100;key = 0;beep(200, 10);break;} // 3
case 0x21 : {prepocetH-=1;key = 0;beep(200, 10);break;} // 4
case 0x22 : {prepocetH-=10;key = 0;beep(200, 10);break;} // 5
case 0x24 : {prepocetH-=100;key = 0;beep(200, 10);break;} // 6
case 0x41 : {prepocetL+=1;key = 0;beep(200, 10);break;} // 7
case 0x42 : {prepocetL+=10;key = 0;beep(200, 10);break;} // 8
case 0x44 : {prepocetL+=100;key = 0;beep(200, 10);break;} // 9
case 0x82 : {prepocetL-=10;key = 0;beep(200, 10);break;} // 0
case 0x81 : {prepocetL-=1;key = 0;beep(200, 10);break;} // *
case 0x84 : {prepocetL-=100;key = 0;beep(200, 10);break;} // #
case 0x28 : {elv+=1000;key = 0;beep(200, 10);break;} // F1
case 0x48 : {elv-=1000;key = 0;beep(200, 10);break;} // F2
case 0x18 : break;
case 0x88 : { // po stisku enter i+1 a ulozi do pole
multi_array[i][0]=prepocetH;
multi_array[i][1]=prepocetL;
multi_array[i][2]=elv;
clear_disp(); //smaze LCD
sprintf(info1, "Zapsana No.: %d°",i+1); //Spoji stringy
put_string_xy(0, 0, info1);
i+=1;
break;
} //ENTER
}
//podminky hlidani
if(prepocetH>65000) prepocetH=0;
if(prepocetL>65000) prepocetL=0;
if(elv>65000) elv=0;
if(prepocetH>=180) prepocetH=180;
if(prepocetL>=180) prepocetL=180;
if(elv>=60000) elv=60000;
//prepocet uhlu na ciselnou hodnotu OCR3A
OCR3A=922+(prepocetL*5.11111);
OCR3B=922+(prepocetH*5.11111);
clear_disp();
sprintf(info4, "Hi: %d°",prepocetH);
sprintf(info2, "Lo: %d°",prepocetL);
sprintf(info3, "t: %dsec",elv/1000);
put_string_xy(0, 0, info4);
cursor_disp_ctrl(0x04);
put_string_xy(8, 0, info2);
put_string_xy(0, 1, info3);
_delay_ms(200);
}
vypis(multi_array[10][3]);
}
//************************************************************************
void vypis(unsigned int multi_array[10][3])
{
char info1[20];
char info4[20];
char info2[20];
char info3[20];
unsigned int z;
unsigned int x;
unsigned int y;
unsigned int j,f;
for (j=0;j<10;j++){ //cyklus cteni z pole a ukladani hodnot do promenych
x=multi_array[j][0];
y=multi_array[j][1];
z=multi_array[j][2];
clear_disp();
sprintf(info4, "Hi: %d°",x);
sprintf(info2, "Lo: %d°",y);
sprintf(info3, "t: %dsec",z/1000);
put_string_xy(0, 0, info4);
cursor_disp_ctrl(0x04);
put_string_xy(8, 0, info2);
put_string_xy(0, 1, info3);
for(f=0;f<10;f++){
if (key==0x18) return(1);
_delay_ms(50);
}
}
return(1);
}
//************************************************************************