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; }