Code:
#include <stdlib.h> #include <avr/io.h>
#include <avr/interrupt.h>
#include "nibo/niboconfig.h"
#include "nibo/iodefs.h"
#include "nibo/delay.h"
#include "nibo/analog.h"
#include "nibo/pwm.h"
#include "nibo/i2cmaster.h"
#include "nibo/display.h"
#include "nibo/bot.h"
#include "nibo/leds.h"
#include "nibo/gfx.h"
#include "nibo/irco.h"
#include "nibo/motco.h"
#include "nibo/floor.h"
void textout(int x, int y, char* str, int ft)
{
gfx_move(x,y);
gfx_set_proportional(ft);
gfx_print_text(str);
}
void Go(int dist, int cm_pro_sec)
{
const float TICKS_PER_CM = 1.75f;
motco_resetOdometry(0,0);
motco_update();
int16_t limit = dist * TICKS_PER_CM;
int16_t pwm = cm_pro_sec * 80;
if (pwm > 1024) pwm = 1024;
if (pwm < -1024) pwm = -1024;
motco_setPWM(pwm,pwm);
motco_setSpeed(cm_pro_sec * TICKS_PER_CM , cm_pro_sec * TICKS_PER_CM);
motco_update();
//delay(3000); //TODO: muss weg
char ticks_r_string[6];
char ticks_l_string[6];
char limit_string[6];
char speed_l_string[6];
char speed_r_string[6];
do
{
motco_update(); /* hoffentlich geradeaus fahren */
textout(0,56," ",0);
itoa(motco_ticks_l,ticks_l_string,10);
textout(0,56,ticks_l_string,0);
itoa(motco_ticks_r,ticks_r_string,10);
textout(25,56,ticks_r_string,0);
itoa(limit,limit_string,10);
textout(50,56,limit_string,0);
itoa(motco_speed_l,speed_l_string,10);
textout(75,56,speed_l_string,0);
itoa(motco_speed_r,speed_r_string,10);
textout(100,56,speed_r_string,0);
delay(200);
}
while(motco_ticks_r < limit);
}
int main()
{
sei(); // enable interrupts
bot_init();
leds_init();
pwm_init();
display_init(2);
gfx_init();
motco_setSpeedParameters(5,6,7);
leds_set_displaylight(800);
textout(10,10,"Nibo meldet:",0);
textout(10,24,"Motor an!",0);
textout(10,38,"Licht an!",0);
leds_set_headlights(512);
int i,j;
for(i=1;i<=6;++i)
{
for(j=0;j<i;++j)
leds_set_status(LEDS_GREEN,j);
Go(10,i);
}
leds_set_headlights(0);
motco_stop();
motco_update();
for(j=0;j<6;++j)
leds_set_status(0,j);
textout(10,24,"Motor aus!",0);
textout(10,38,"Licht aus!",0);
while(1);
return 0;
}
Die Werte für ticks oder speed bleiben bei 0.
Lesezeichen