void Move_Motor( void *pvParameters ) {
MotorsInit();
tMotorMove* motorActions = (tMotorMove*) pvParameters;
unsigned char actionIndex = 0;
while( motorActions[actionIndex].duration != 0 ) {
MotorDir( LEFT_SIDE, motorActions[actionIndex].leftDirection );
MotorDir( RIGHT_SIDE, motorActions[actionIndex].rightDirection );
MotorSpeed( LEFT_SIDE, motorActions[actionIndex].leftSpeed);
MotorSpeed( RIGHT_SIDE, motorActions[actionIndex].rightSpeed);
MotorRun(LEFT_SIDE);
MotorRun(RIGHT_SIDE);
vTaskDelay( motorActions[actionIndex].duration );
actionIndex++;
}
MotorStop(LEFT_SIDE);
MotorStop(RIGHT_SIDE);
while (1) {
vTaskDelay( 500 );
}
}
int bcr_scanIrregularLines(unsigned char num, unsigned int spacing)
{
/* maxlength = 100 means that it will drive max. 100 ticks
* to find the first bar! Later in the code it will contain
* the regular space between the bars. */
unsigned int whiteLength = 200;
if (spacing < whiteLength) { spacing = whiteLength; } /* default buffer */
// unsigned int blackLength = 200;
lineCounter = 0;
/* assume we stand on white (or not completely dark) */
currentState = BCR_DIMMING_STATE;
EncoderSet(0,0);
MotorSpeed(READ_SPEED, READ_SPEED);
while(1) {
switch (currentState)
{
case BCR_DIMMING_STATE:
EncoderSet(0,0);
while (bcr_getGradient() >= -1*M_TOL*tolFactor) {
if ( encoder[LEFT] > (whiteLength+spacing) ) {
MotorDir(BREAK, BREAK);
MotorSpeed(0,0);
return lineCounter;
}
}
if (lineCounter >= 1) { whiteLength = encoder[LEFT]; }
currentState = BCR_BRIGHTEN_STATE;
lineCounter += 1;
// util_intToBeep(1);
// MotorSpeed(READ_SPEED, READ_SPEED);
break;
case BCR_BRIGHTEN_STATE:
EncoderSet(0,0);
while (bcr_getGradient() <= M_TOL*tolFactor) {
// if ( encoder[LEFT] > (2*whiteLength+spacing) ) {
// MotorDir(BREAK, BREAK);
// MotorSpeed(0,0);
// return lineCounter;
// }
}
currentState = BCR_DIMMING_STATE;
break;
}
/* enough lines scanned? -> stop */
if (num>0 && lineCounter >= num) {
MotorDir(BREAK, BREAK);
// MotorDir(RWD, RWD);
GoTurn(-5, 0, READ_SPEED);
break;
}
} // end switch
MotorSpeed(0,0);
return lineCounter;
}
//go up
int motor_UP(){
evalbot->ord = UP;
MotorDir(LEFT_SIDE,FORWARD);
MotorDir(RIGHT_SIDE,FORWARD);
MotorRun(LEFT_SIDE);
MotorRun(RIGHT_SIDE);
return 1;
}
//turn L 90
int motor_RIGHT(){
evalbot->ord = RIGHT;
MotorDir(LEFT_SIDE,FORWARD);
MotorDir(RIGHT_SIDE,FORWARD);
MotorDir(RIGHT_SIDE,REVERSE);
MotorRun(LEFT_SIDE);
MotorRun(RIGHT_SIDE);
return 1;
}
void Go(int16_t distance, uint8_t speed)
{
if(distance>0) MotorDir(FWD, FWD);
else MotorDir(RWD, RWD);
MotorSpeed(speed,speed);
delay((int32_t)abs(distance)*DISTANCE_FACTOR);
MotorDir(BREAK,BREAK); // rotate if there is no signal
delay(200);
}
void Turn(int16_t degree, uint8_t speed)
{
if(degree>0) MotorDir(FWD, RWD);
else MotorDir(RWD, FWD);
MotorSpeed(speed,speed);
delay((int32_t)abs(degree)*TURN_FAKTOR/10);
MotorDir(BREAK,BREAK);
delay(200);
}
//go Down
int motor_DOWN(){
evalbot->ord = BACK;
MotorDir(LEFT_SIDE,FORWARD);
MotorDir(RIGHT_SIDE,FORWARD);
MotorDir(LEFT_SIDE,REVERSE);
MotorDir(RIGHT_SIDE,REVERSE);
MotorRun(LEFT_SIDE);
MotorRun(RIGHT_SIDE);
return 1;
}
int sterowanie(tDirection kierunek,int pwmL,int pwmP)
{
MotorDir((tSide)1,(tDirection)kierunek);
MotorDir((tSide)0,(tDirection)kierunek);
MotorSpeed((tSide)0, pwmL << 8);
MotorSpeed((tSide)1, pwmP << 8);
cofanie=0;
MotorRun((tSide)0);
MotorRun((tSide)1);
return 0;
}
int test04(void)
{
int n;
Init();
StatusLED(XRD);
FrontLED(ON);
BackLED(ON, ON);
MotorDir(FREE, FREE);
MotorSpeed(150, 150);
for (n=1; n<200; n++) Sleep(255);
StatusLED(XGN);
FrontLED(OFF);
BackLED(OFF, OFF);
for (n=1; n<50; n++) Sleep(255);
while(1)
{
StatusLED(XGN);
BackLED(OFF, OFF);
FrontLED(ON);
MotorDir(FWD, FWD);
for (n=1; n<500; n++) Sleep(255);
StatusLED(XRD);
BackLED(OFF, OFF);
FrontLED(OFF);
MotorDir(BREAK, BREAK);
for (n=1; n<100; n++) Sleep(255);
StatusLED(XGN);
BackLED(ON, ON);
FrontLED(OFF);
MotorDir(RWD, RWD);
for (n=1; n<500; n++) Sleep(255);
StatusLED(XRD);
BackLED(OFF, OFF);
FrontLED(OFF);
MotorDir(BREAK, BREAK);
for (n=1; n<100; n++) Sleep(255);
}
return 0;
}
int test03(void)
{
int n;
Init();
StatusLED(XGN);
FrontLED(ON);
BackLED(ON, ON);
MotorDir(BREAK, FREE);
MotorSpeed(150,80);
while(1)
{
BackLED(OFF, OFF);
StatusLED(XGN);
for (n=1; n<100; n++) Sleep(255);
StatusLED(XRD);
for (n=1; n<100; n++) Sleep(255);
}
return 0;
}
/*!
\brief
Steuert die Motorgeschwindigkeit und Drehrichtung der Motoren.
\param[in]
leftpwm linker Motor (-rückwaerts, + vorwaerts) (Wertebereich -128...127)
\param[in]
rightpwm rechter Motor (-rückwaerts, + vorwaerts) (Wertebereich -128...127)
\par Hinweis:
Da der Wertebereich dieser Funktion nur von -128 bis +127 reicht, aber die\n
urspruengliche Funktion MotorSpeed() einen Wertebereich bis +255 hat, werden\n
die hier uebergebene Parameter als Absolutwert mit 2 multipliziert\n
weitergegeben.\n
\par Beispiel:
(Nur zur Demonstration der Parameter/Returnwerte)
\code
// Setzt die Geschwindigkeit fuer den linken Motor auf 60 (vorwaerts),
// und für den rechten Motor auf -60 (rueckwaerts)
// Asuro soll auf der Stelle drehen.
SetMotorPower (60, -600);
\endcode
*****************************************************************************/
void SetMotorPower (
int8_t leftpwm,
int8_t rightpwm)
{
unsigned char left, right;
if (leftpwm < 0) // Ein negativer Wert fuehrt ...
{
left = RWD; // ... zu einer Rueckwaertsfahrt, ...
leftpwm = -leftpwm; // aber immer positiv PWM-Wert
}
else
left = FWD; // ... sonst nach vorne, ...
if (leftpwm == 0)
left = BREAK; // ... oder bei 0 zum Bremsen.
if (rightpwm < 0)
{
right = RWD;
rightpwm = -rightpwm;
}
else
right = FWD;
if (rightpwm == 0)
right = BREAK;
MotorDir (left, right); // Drehrichtung setzen
/*
Die Geschwindigkeitsparameter mit 2 multiplizieren, da der Absolutwert
der Parameter dieser Funktion nur genau die Haelfte von der MotorSpeed()-
Funktion betraegt.
*/
MotorSpeed (leftpwm * 2, rightpwm * 2);
}
void Lichtorgel(void){
MotorDir(FWD,BWD);
MotorSpeed(255,255);
for(int q = 0; q < 15; q++){
StatusLED(GREEN);
msleep(50);
BackLED(ON,OFF);
msleep(50);
StatusLED(RED);
msleep(50);
BackLED(OFF,ON);
msleep(50);
StatusLED(YELLOW);
msleep(50);
FrontLED(ON);
msleep(50);
FrontLED(OFF);
}
MotorDir(BREAK,BREAK);
MotorSpeed(0,0);
while(1){
StatusLED(GREEN);
msleep(50);
BackLED(ON,OFF);
msleep(50);
StatusLED(RED);
msleep(50);
BackLED(OFF,ON);
msleep(50);
StatusLED(YELLOW);
msleep(50);
FrontLED(ON);
msleep(50);
FrontLED(OFF);
}
}
int test05(void)
{
int n, s, light;
char key;
Init();
StatusLED(XGN);
FrontLED(ON);
BackLED(ON, ON);
//SerWrite("Begin...", 8);
//SerWrite("Waiting for command... ", 23);
SerRead(key, 1, 0);
if (key == 'g')
{
SerWrite("Good Morning... Starting...", 27);
}
else
{
SerWrite("So... just start... ", 20);
}
while(1)
{
for (s=80; s<300; s+=10)
{
SerWrite("Looping... ", 11);
MotorDir(FWD, RWD);
MotorSpeed(150,s);
SerWrite("Sleep Now! ", 11);
for (n=1; n< 100; n++) Sleep(255);
if (s > 200)
{
SerWrite("S>200 ", 6);
s=80;
}
}
}
return 0;
}
int test01(void)
{
Init();
StatusLED(XGN);
FrontLED(ON);
BackLED(ON, ON);
MotorDir(BREAK, FREE);
MotorSpeed(150,80);
while(1);
return 0;
}
int test02(void)
{
Init();
StatusLED(XGN);
FrontLED(ON);
BackLED(ON, ON);
MotorDir(FWD, RWD);
MotorSpeed(150,80);
while(1);
return 0;
}
extern "C" void PIOINT1_IRQHandler(void)
{
U64 systickcnt = SysTickCnt;
if (GPIO1_MIS(0))
{
g_motorPos += g_motorInc;
g_motorTime = systickcnt;
if (g_motorPos == g_motorStop)
{
MotorDir(0, 0); // brake motor
MotorPower(0, 100); // stop the motor
}
GPIO1_IC(0); // clear the interrupt flag
__NOP();__NOP(); // required after clearing interrupt flags
}
}
MAIN void main(void) {
Init();
// Enable odometry
LED_CONFIGURE_ODOMETRY();
// Configure auto measurment mode
SensorInit();
SensorConfigAutomode(am_odo);
// Setup encoder control
EncoderInit();
EncoderMovementReset();
// Initialize 1kHz tick timer
Timer0Init();
Timer0IntEnable();
// Set drive direction
MotorDir(FWD, FWD);
for (;;) {
// Circle with approx. 10cm diameter
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
EncoderMovementSetSpeed(INNER_SPEED, OUTER_SPEED(100));
}
msleep(5000);
// Circle with approx. 25cm diameter
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
EncoderMovementSetSpeed(INNER_SPEED, OUTER_SPEED(250));
}
msleep(3000);
// Tightening spiral
for (uint8_t i = 250; i >= 80; i -= 10) {
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
EncoderMovementSetSpeed(INNER_SPEED, OUTER_SPEED(i));
}
msleep(500);
}
// Straight line
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
EncoderMovementSetSpeed(125, 125);
}
msleep(3000);
}
}
void setMotor(char DirLeft, char DirRight, unsigned char SpeedLeft, unsigned char SpeedRight)
{
IstSpeedLeft = constrain(IstSpeedLeft, 100, 255);
IstSpeedRight = constrain(IstSpeedRight, 100, 255);
/*usart_puts("setMotor");
usart_putc('L');
usart_puti((int) SpeedLeft, 3);
usart_putc('R');
usart_puti((int) SpeedRight, 3);*/
MotorDir(DirLeft,DirRight);
//usart_puts("\r\n");
MotorSpeed(SpeedLeft, SpeedRight);
return;
}
void bcr_initBarcodeReader(void) {
EncoderInit();
MotorDir(FWD,FWD);
FrontLED(ON);
lineCounter = 0;
rawCounter = 0;
latestLineData = 0;
unsigned int tmp[2];
util_getStableLineData(tmp);
tolFactor = getToleranceValue(tmp[LEFT] + tmp[RIGHT]);
int i;
util_pauseInterrupts();
for(i = 0; i < (M_RAW_DATAPOINTS * M_WINDOW_SIZE); i++) {
measureDataPoint();
Msleep(5);
}
ts_init();
ts_addFunction(&measureDataPoint, M_SAMPLING_FREQ);
sei();
}
/* Init function Processor will be initalized to work correctly */
void Init (void)
{
//-------- seriell interface programmed in boot routine and already running -------
// prepare 36kHz for IR - Communication
TCCR2 = (1 << WGM21) | (1 << COM20) | (1 << CS20);
OCR2 = 0x6E; // 36kHz @8MHz
TIMSK |= (1 << OCIE2); // 36kHz counter for sleep
// prepare RS232
UCSRA = 0x00;
UCSRB = 0x00;
UCSRC = 0x86; // No Parity | 1 Stop Bit | 8 Data Bit
UBRRL = 0xCF; // 2400bps @ 8.00MHz
// I/O Ports
DDRB = IRTX | LEFT_DIR | PWM | GREEN_LED;
DDRD = RIGHT_DIR | FRONT_LED | ODOMETRIE_LED | RED_LED;
// for PWM (8-Bit PWM) on OC1A & OC1B
TCCR1A = (1 << WGM10) | (1 << COM1A1) | (1 << COM1B1);
// tmr1 running on MCU clock/8
TCCR1B = (1 << CS11);
// A/D Conversion
ADCSRA = (1 << ADEN) | (1 << ADPS2) | (1 << ADPS1); // clk/64
ODOMETRIE_LED_OFF;
FrontLED(OFF);
BackLED(ON,ON);
BackLED(OFF,OFF);
StatusLED(GREEN);
MotorDir(FWD,FWD);
MotorSpeed(0,0);
sei();
}
void StereoSound(uint16_t *noten1, uint16_t *noten2)
{
uint16_t index1=0,index2=0;
uint16_t timer1,timer2;
uint16_t phase1,phase2,angle1,angle2;
uint8_t dir1=FWD,dir2=FWD;
uint8_t speed1,speed2;
FrontLED(OFF);
BackLED(OFF,OFF);
StatusLED(OFF);
angle1=(uint32_t)noten1[index1++]*65536/FS;
timer1=noten1[index1++]*(FS/1000);
if(angle1==0)speed1=0;
else speed1=255;
angle2=(uint32_t)noten2[index2++]*65536/FS;
timer2=noten2[index2++]*(FS/1000);
if(angle2==0)speed2=0;
else speed2=255;
MotorSpeed(speed1,speed2);
cli(); // stop all interrupts
while(timer1!=0)
{
timer1--;
timer2--;
if(timer1==0)
{
angle1=(uint32_t)noten1[index1++]*65536/FS;
timer1=noten1[index1++]*(FS/1000);
if(angle1==0)speed1=0;
else speed1=255;
MotorSpeed(speed1,speed2);
}
if(timer2==0)
{
angle2=(uint32_t)noten2[index2++]*65536/FS;
timer2=noten2[index2++]*(FS/1000);
if(angle2==0)speed2=0;
else speed2=255;
MotorSpeed(speed1,speed2);
}
phase1+=angle1;
if(phase1&0x8000) dir1=FWD;
else dir1=RWD;
phase2+=angle2;
if(phase2&0x8000) dir2=FWD;
else dir2=RWD;
MotorDir(dir1,dir2);
// sync with Timer2 ( 31250Hz )
while(!(TIFR&(1<<TOV2)));
TIFR|=TIFR&(1<<TOV2);
}
MotorSpeed(0,0);
sei(); // enable all interrupts
}
void main(void) {
Init();
FrontLED(ON);
int i=0;
int linienEnde = 0;
int fast = 200;
unsigned int stData = 0, schwarz = 0;
//Weißwert festlegen
unsigned int data[2];
LineData(data);
stData = data[LEFT] + data[RIGHT] + 50;
StatusLED(GREEN);
msleep(3000);
StatusLED(YELLOW);
//Schwarzwert setzen
LineData(data);
schwarz = data[LEFT] + data[RIGHT];
msleep(1000);
StatusLED(GREEN);
MotorSpeed(fast,fast);
StatusLED(YELLOW);
while(1){
LineData(data);
if(data[LEFT] > data[RIGHT]){
MotorDir(FWD,FREE);
BackLED(ON,OFF);
}else if(data[RIGHT] > data[LEFT]){
MotorDir(FREE,FWD);
BackLED(OFF,ON); ;
}
if(data[LEFT]+data[RIGHT]>stData){
StatusLED(RED);
i++;
}else{
i=0;
StatusLED(YELLOW);
}
if(i>100){
linienEnde = 1;
}
while(linienEnde){
MotorDir(FWD,RWD);
LineData(data);
if(data[LEFT]+data[RIGHT] < schwarz+100){
MotorDir(BREAK,BREAK);
msleep(50);
while(linienEnde){
MotorSpeed(80,80);
MotorDir(RWD,FWD);
LineData(data);
if(data[LEFT] + data[RIGHT] < schwarz+100){
linienEnde = 0;
}
}
MotorSpeed(fast,fast);
}
}
Kollision();
}
}
/*!
\brief
Initialisiert die Hardware: Ports, A/D Wandler, Serielle Schnittstelle, PWM\n
Die Init Funktion muss von jeden Programm beim Start aufgerufen werden
\see
Die Funktionen Sleep() und Msleep() in time.c werden mit dem hier\n
eingestellten 36 kHz-Takt betrieben.\n
\par Funktionsweise der Zeitfunktionen:
Msleep() ruft Sleep() auf. In Sleep() wird die globale Variable count36kHz\n
zur Zeitverzoegerung benutzt. Diese Variable wird jedesmal im Interrupt\n
SIG_OVERFLOW2 um 1 hochgezaehlt.\n
Der Interrupt selber wird durch den hier eingestellten Timer ausgeloesst.\n
Somit ist dieser Timer fuer die Zeitverzoegerung zustaendig.
\see
Die globale Variable autoencode fuer die automatische Bearbeitung der\n
Odometrie-ADC-Wandler wird hier auf FALSE gesetzt.
\par Hinweis zur 36 kHz-Frequenz vom Timer 2
Genau diese Frequenz wird von dem Empfaengerbaustein benoetigt und kann\n
deshalb nicht geaendert werden.\n
In der urspruenglichen, vom Hersteller ausgelieferten LIB, war diese\n
Frequenz allerdings auf 72 kHz eingestellt. Durch eine geschickte\n
Umkonfigurierung durch waste konnte diese aber halbiert werden.\n
Sinnvoll ist dies, da der durch diesen Timer2 auch ausgeloesste Timer-\n
Interrupt dann nur noch die Haelfte an Rechenzeit in Anspruch nimmt.
\par Beispiel:
(Nur zur Demonstration der Parameter/Returnwerte)
\code
// Die Init()-Funktion MUSS IMMER zu Anfang aufgerufen werden.
int main (void)
{
int wert;
Init ();
while (1)
(
// Dein Programm
}
return 0;
}
\endcode
*****************************************************************************/
void Init (
void)
{
/*
Timer2, zum Betrieb mit der seriellen Schnittstelle, fuer die
IR-Kommunikation auf 36 kHz eingestellt.
*/
#if defined(__AVR_ATmega168__)
// fast PWM, set OC2A on compare match, clear OC2A at bottom, clk/1
TCCR2A = _BV(WGM20) | _BV(WGM21) | _BV(COM2A0) | _BV(COM2A1);
TCCR2B = _BV(CS20);
// interrupt on timer overflow
TIMSK2 |= _BV(TOIE2);
#else
// fast PWM, set OC2A on compare match, clear OC2A at bottom, clk/1
TCCR2 = _BV(WGM20) | _BV(WGM21) | _BV(COM20) | _BV(COM21) | _BV(CS20);
// interrupt on timer overflow
TIMSK |= _BV(TOIE2);
#endif
// 36kHz carrier/timer
OCR2 = 0x91;
/*
Die serielle Schnittstelle wurde waerend der Boot-Phase schon
programmiert und gestartet. Hier werden die Parameter auf 2400 1N8 gesetzt.
*/
#if defined(__AVR_ATmega168__)
UBRR0L = (uint8_t)(F_CPU/(BAUD_RATE*16L)-1);
UBRR0H = (F_CPU/(BAUD_RATE*16L)-1) >> 8;
UCSR0B = (1<<RXEN0) | (1<<TXEN0);
UCSR0C = (1<<UCSZ00) | (1<<UCSZ01);
#else
UBRRH = (((F_CPU/BAUD_RATE)/16)-1)>>8; // set baud rate
UBRRL = (((F_CPU/BAUD_RATE)/16)-1);
UCSRB = (1<<RXEN)|(1<<TXEN); // enable Rx & Tx
UCSRC = (1<<URSEL)|(1<<UCSZ1)|(1<<UCSZ0); // config USART; 8N1
#endif
/*
Datenrichtung der I/O-Ports festlegen. Dies ist durch die Beschaltung der
Asuro-Hardware nicht aenderbar.
Port B: Seriell Senden; Richtungsvorgabe Motor links; Takt fuer die
Geschwindigkeit beider Motoren; Grueneanteil-Status-LED
Port D: Richtungsvorgabe Motor rechts; Vordere LED;
Odometrie-LED (Radsensor); Rotanteil-Status-LED
*/
DDRB = IRTX | RIGHT_DIR | PWM | GREEN_LED;
DDRD = LEFT_DIR | FRONT_LED | ODOMETRIE_LED | RED_LED;
/*
PWM-Kanaele OC1A und OC1B auf 8-Bit einstellen.
Sie werden fuer die Geschwindigkeitsvorgaben der Motoren benutzt.
*/
TCCR1A = _BV(WGM10) | _BV(COM1A1) | _BV(COM1B1);
TCCR1B = _BV(CS11); // tmr1-Timer mit MCU-Takt/8 betreiben.
/*
Einstellungen des A/D-Wandlers auf MCU-Takt/64
*/
ADCSRA = _BV(ADEN) | _BV(ADPS2) | _BV(ADPS1);
/*
Sonstige Vorbereitungen.
- Alle LED's ausschalten
- Motoren stoppen und schon mal auf Vorwaerts einstellen.
- Globale Variable autoencoder ausschalten.
*/
ODOMETRIE_LED_OFF;
FrontLED (OFF);
BackLED (ON, ON);
BackLED (OFF, OFF);
StatusLED (GREEN);
MotorDir (FWD, FWD);
MotorSpeed (0, 0);
autoencode = FALSE;
Ovr2IntFunc = 0;
AdcIntFunc = 0;
/*
Funktion zum ALLGEMEINEN ZULASSEN von Interrupts.
*/
sei ();
}
/*!
\brief
Faehrt eine bestimmte Strecke mit einer bestimmten Geschwindigkeit.
(Autor: stochri)\n
ODER\n
Dreht um einen bestimmten Winkel mit einer bestimmten Geschwindigkeit.
(Autor: stochri)\n
Benutzt die Odometrie Sensoren im Interrupt Betrieb.\n
Vor dem ersten Aufruf muss deshalb EncoderInit() aufgerufen werden.
\param[in]
distance Distanz in mm (- rueckwaerts, + = vorwaerts)\n
Bei 0 wird degree fuer einen Turn benutzt.
\param[in]
degree Winkel (- rechts, + links)
\param[in]
speed Geschwindigkeit (Wertebereich 0...255)
\return
nichts
\see
MACRO Go() und MACRO Turn() koennen weiterhin aufgerufen werden um bestehenden
Programmcode nicht umschreiben zu muessen.
\see
In der globale Variable encoder, werden die Hell-/Dunkelwechsel der\n
Encoderscheiben im Interruptbetrieb gezaehlt.
\par Hinweis:
Die Berechnung der zu fahrenden Ticks beruht auf der Annahme, dass die\n
Anzahl der schwarzen Teilstuecke und die Raddurchmesser wie bei stochri sind.\n
(Sternthaler) Vermutung, dass der Hersteller unterschiedlich grosse Raeder\n
ausgeliefert hat, da die Berechnung in dieser Form bei Sternthaler nicht\n
funktioniert.
\par Beispiel:
(Nur zur Demonstration der Parameter/Returnwerte)
\code
// Laesst den Asuro ein Quadrat von 200 mm fahren,
// bei einer Geschwindigkeit von 150.
EncoderInit ();
for (i = 0; i < 4; i++)
{
GoTurn (200, 0, 150); // entspricht Go (200, 150)
GoTurn ( 0, 90, 150); // entspricht Turn (90, 150)
}
\endcode
*****************************************************************************/
void GoTurn (
int distance,
int degree,
int speed)
{
unsigned long enc_count;
int tot_count = 0;
int diff = 0;
int l_speed = speed, r_speed = speed;
/* stop the motors until the direction is set */
MotorSpeed (0, 0);
/* if distance is NOT zero, then take this value to go ... */
if (distance != 0)
{
/* calculate tics from mm */
enc_count = abs (distance) * 10000L;
enc_count /= MY_GO_ENC_COUNT_VALUE;
if (distance < 0)
MotorDir (RWD, RWD);
else
MotorDir (FWD, FWD);
}
/* ... else take the value degree for a turn */
else
{
/* calculate tics from degree */
enc_count = abs (degree) * MY_TURN_ENC_COUNT_VALUE;
enc_count /= 360L;
if (degree < 0)
MotorDir (RWD, FWD);
else
MotorDir (FWD, RWD);
}
/* reset encoder */
EncoderSet (0, 0);
/* now start the machine */
MotorSpeed (l_speed, r_speed);
while (tot_count < enc_count)
{
tot_count += encoder [LEFT];
diff = encoder [LEFT] - encoder [RIGHT];
if (diff > 0)
{ /* Left faster than right */
if ((l_speed > speed) || (r_speed > 244))
l_speed -= 10;
else
r_speed += 10;
}
if (diff < 0)
{ /* Right faster than left */
if ((r_speed > speed) || (l_speed > 244))
r_speed -= 10;
else
l_speed += 10;
}
/* reset encoder */
EncoderSet (0, 0);
MotorSpeed (l_speed, r_speed);
Msleep (1);
}
MotorDir (BREAK, BREAK);
Msleep (200);
}
int main(void)
{
// Set system frequency to 48MHz
SystemFrequency = ConfigurePLL(12000000UL, 48000000UL);
// Enable clock to IO Configuration block. Needed for UART, SPI, I2C, etc...
SYSCON_SYSAHBCLKCTRL_IOCON(SYSAHBCLKCTRL_ENABLE);
SysTick_Config(SystemFrequency/10000 - 1); // Generate interrupt each .1 ms
// set direction on port 0_7 (pin 28) to output
GPIO0_DIR(7, GPIO_OUTPUT);
GPIO0_DATA(7, 1); // turn on diagnostic led
GPIO1_DIR(4, GPIO_OUTPUT); // configure port 1_5 (pin 14) for output
GPIO1_DIR(5, GPIO_OUTPUT); // configure port 1_5 (pin 14) for output
// PWM output (pin 1) is tied to 1,2EN on H-bridge (SN754410 Quad Half H-bridge) //
SetupPWM();
MotorDir(0, 0); // turn on braking at 100%
MotorPower(0, 100);
// motor details:
// 120:1 gearbox
// 8 teeth on motor gear
// 32 teeth on gear with optical wheel
// 4 edges per rot of optical wheel
// 8/32*120*4 = 106.666 edges per rev
// At 5.5V max speed with just 2 gears,
// pulse width is avg 9.5 ms.
//
// a full stop should be no edge for about 150ms
// and motor on brake.
//
g_motorPos = 0;
g_motorStop = 0;
g_motorTime = 0;
g_motorInc = 0;
// setup interrupts on PIO1_0
IOCON_PIO1_0_MODE(PIO1_0_MODE_PULLDOWN_RESISTOR);
GPIO1_DIR(0, GPIO_INPUT);
GPIO1_IS(0, GPIO_EDGE_SENSITIVE);
GPIO1_IBE(0, GPIO_BOTH_EDGES);
GPIO1_IE(0, GPIO_INTERRUPT_ENABLE);
NVIC_EnableIRQ(EINT1_IRQn);
U64 time1, time2, oldTime;
int pos, oldPos;
oldTime = 0xFFFFFFFFFFFFFFFF;
U64 systickcnt;
int desiredMotorInc; // g_motorInc can only be changed when in a stopped state
while (1)
{
FlashLED();
desiredMotorInc = g_motorInc = 1;
g_motorStop = 100;
MotorDir(0, 1);
MotorPower(0, 100);
while (1)
{
do {
time1 = g_motorTime;
pos = g_motorPos;
time2 = g_motorTime;
} while (time1 != time2);
if (pos > g_motorStop)
{
desiredMotorInc = -1;
}
else if (pos < g_motorStop)
{
desiredMotorInc = 1;
}
systickcnt = SysTickCnt;
if ((systickcnt - time2) > 200 /*ms*/)
{
// stopped
if (pos == g_motorStop)
{
break; //achieved goal pos and we are stopped
}
if (desiredMotorInc != g_motorInc)
{
g_motorInc = desiredMotorInc; // g_motorInc must always be 1 or -1, otherwise we lose track of position
// change direction and turn power back on
MotorDir(0, desiredMotorInc);
MotorPower(0, 20);
}
}
}
FlashLED();
desiredMotorInc = g_motorInc = -1;
g_motorStop = 0;
MotorDir(0, -1);
MotorPower(0, 100);
while (1)
{
do {
time1 = g_motorTime;
pos = g_motorPos;
time2 = g_motorTime;
} while (time1 != time2);
if (pos > g_motorStop)
{
desiredMotorInc = -1;
}
else if (pos < g_motorStop)
{
desiredMotorInc = 1;
}
systickcnt = SysTickCnt;
if ((systickcnt - time2) > 200 /*ms*/)
{
// stopped
if (pos == g_motorStop)
{
break; //achieved goal pos and we are stopped
}
if (desiredMotorInc != g_motorInc)
{
g_motorInc = desiredMotorInc; // g_motorInc must always be 1 or -1, otherwise we lose track of position
// change direction and turn power back on
MotorDir(0, desiredMotorInc);
MotorPower(0, 20);
}
}
}
}
}
int main()
{
unsigned char ctl, engineLeft, engineRight;
// do ASURO initialization
Init();
// initialize control value
ctl = 100;
// loop forever
while (1)
{
// generate new speed levels for left and right engine
engineLeft = (unsigned char)randomValue(ctl);
engineRight = (unsigned char)randomValue(engineLeft);
// go straight forward when ctl is between 0 and 31
if (ctl <32)
{
MotorDir(FWD,FWD);
MotorSpeed(engineLeft,engineLeft);
}
// go a curve in forward direction in case that ctl is between 32 and 63
else if (ctl <64)
{
MotorDir(FWD,FWD);
MotorSpeed(engineLeft,engineRight);
}
// cycle right when ctl between 64 and 127
else if (ctl <128)
{
MotorDir(FWD, RWD);
MotorSpeed(engineLeft,engineRight);
}
// cycle left when ctl is between 128 and 191
else if (ctl <192)
{
MotorDir(RWD, FWD);
MotorSpeed(engineLeft,engineRight);
}
// go straight back when ctl is between 192 and 223
else if (ctl <224)
{
MotorDir(RWD,RWD);
MotorSpeed(engineRight,engineRight);
}
// go straight back in case that ctl is between 224 and 255
else
{
MotorDir(RWD,RWD);
MotorSpeed(engineRight,engineRight);
}
// drive for the number of milliseconds specified by five-fold ctl
Msleep(5*ctl);
// generate a new control word
ctl = (unsigned char)randomValue(engineRight);
}
return -1;
}
