예제 #1
0
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 );
	}
}
예제 #2
0
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;
}
예제 #3
0
//go up 
int motor_UP(){
	evalbot->ord = UP;
	MotorDir(LEFT_SIDE,FORWARD);
	MotorDir(RIGHT_SIDE,FORWARD);
	MotorRun(LEFT_SIDE);
	MotorRun(RIGHT_SIDE);
	return 1;
}
예제 #4
0
//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;
}
예제 #5
0
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);
}
예제 #6
0
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);
}
예제 #7
0
//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;
}
예제 #8
0
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;
}
예제 #9
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;
}
예제 #10
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;
}
예제 #11
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);
}
예제 #12
0
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);
	}
}
예제 #13
0
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;
}
예제 #14
0
int test01(void)
{
    Init();

    StatusLED(XGN);
    
    FrontLED(ON);
    BackLED(ON, ON);

    MotorDir(BREAK, FREE);
    MotorSpeed(150,80);

    while(1);
    return 0;
}
예제 #15
0
int test02(void)
{
    Init();

    StatusLED(XGN);

    FrontLED(ON);
    BackLED(ON, ON);

    MotorDir(FWD, RWD);
    MotorSpeed(150,80);

    while(1);
    return 0;
}
예제 #16
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
	}
}
예제 #17
0
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);
    }
}
예제 #18
0
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;  
}
예제 #19
0
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();
}
예제 #20
0
/* 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();
}
예제 #21
0
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
}
예제 #22
0
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();
	}
}
예제 #23
0
/*!
  \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 ();
}
예제 #24
0
/*!
  \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);
}
예제 #25
0
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);
				}
			}
		}

	}
}
예제 #26
0
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;
}