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;
}
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 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 MotorDir(uint8_t left, uint8_t right)
{
//*************** left motor *****************
if (left == FWD)
{
digitalWrite(MOTOR_LEFT1, LOW);
digitalWrite(MOTOR_LEFT2, HIGH);
}
if (left == RWD)
{
digitalWrite(MOTOR_LEFT1, HIGH);
digitalWrite(MOTOR_LEFT2, LOW);
}
if (left == BREAK)
{
digitalWrite(MOTOR_LEFT1, LOW);
digitalWrite(MOTOR_LEFT2, LOW);
MotorSpeed(255, 255);
}
if (left == FREE)
{
MotorSpeed(0, 0);
}
//********** right motor ********************
if (right == FWD)
{
digitalWrite(MOTOR_RIGHT1, LOW);
digitalWrite(MOTOR_RIGHT2, HIGH);
}
if (right == RWD)
{
digitalWrite(MOTOR_RIGHT1, HIGH);
digitalWrite(MOTOR_RIGHT2, LOW);
}
if (right == BREAK)
{
digitalWrite(MOTOR_RIGHT1, LOW);
digitalWrite(MOTOR_RIGHT2, LOW);
MotorSpeed(255, 255);
}
if (right == FREE)
{
MotorSpeed(0, 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);
}
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);
}
// SETUP
void setup()
{
// For algorithm speed checking
loopPeriod = 0;
lastLoop = micros();
// Lap timer
lapTimer.setInterval(100, IncrementTime);
lapTimer.disable(0);
// Initialize LCD
lcd.begin(16, 2);
lcd.setCursor(0,0);
// Turn signals
pinMode(LEFT_TURN_LED, OUTPUT);
pinMode(RIGHT_TURN_LED, OUTPUT);
digitalWrite(LEFT_TURN_LED, 0);
digitalWrite(RIGHT_TURN_LED, 0);
turnSignalTimer.setInterval(50, TurnSignal);
// Force motors off by default
MotorSpeed(LEFT_MOTOR, 0);
MotorSpeed(RIGHT_MOTOR, 0);
// Parameters need to be loaded from EEPROM
LoadFromEEPROM();
// Intro text
Clear();
Cursor(TOP, 0);
Print("FAST ORANGE");
Cursor(BOTTOM, 0);
#ifdef DEBUG_MODE
Print("Debug Mode");
#else
Print("Race Mode");
#endif
delay(1000);
currentState = menu;
}
void ProcessMovementAnalog()
{
if (!leftDetected && !rightDetected) error = (previousError <= 0) ? -500 : 500;
else error = float(left) - float(right);
// Proportional
proportional = error * float(proportionalGain.Value) / 10.0;
derivative = (error - previousError) * float(derivativeGain.Value) / 50.0;
integral += (error * float(integralGain.Value)) / 50.0;
if (integral > integralCap.Value)
integral = integralCap.Value;
else if (integral < -integralCap.Value)
integral = -integralCap.Value;
#ifndef CALCULATE_INTEGRAL_ERROR
integral = 0;
#endif;
#ifndef CALCULATE_DERIVATIVE_ERROR
derivative = 0;
#endif;
int baseSpeed;
if (center > centerSchmittHigh.Value)
baseSpeed = 1000;
else
baseSpeed = (speed.Value * 4.0);
int mLeft = baseSpeed + (proportional + derivative + integral);
int mRight = baseSpeed - (proportional + derivative + integral);
#ifdef SUBTRACTIVE_MOTOR_SPEED
if (mLeft > mRight)
mLeft = baseSpeed;
else
mRight = baseSpeed;
#endif
MotorSpeed(LEFT_MOTOR, mLeft);
MotorSpeed(RIGHT_MOTOR, mRight);
}
void Turn()
{
turnSignalState = true;
TurnSignal();
int direction;
if (currentState == turnLeft) direction = 1;
else if (currentState == turnRight) direction = -1;
else return;
Clear();
Print("TURN");
MotorSpeed(LEFT_MOTOR, 1000 * direction);
MotorSpeed(RIGHT_MOTOR, 1000 * -direction);
delay(4.0 * turnTime.Value);
previousError = direction;
integral = 0;
currentState = moveStraight;
Clear();
}
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;
}
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);
}
}
// LOOP
void loop()
{
switch(currentState)
{
case menu:
MotorSpeed(LEFT_MOTOR, 0);
MotorSpeed(RIGHT_MOTOR, 0);
ShowMenu();
break;
case moveStraight:
Update();
ProcessMovementAnalog();
break;
case turnLeft:
case turnRight:
Turn();
break;
}
turnSignalTimer.run();
lapTimer.run();
}
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;
}
// Updates the sensors and machine state
void Update()
{
if ((lcdRefreshCount == 0) && (currentState != menu) && (ReadButton() == SELECT))
{
delay(750);
if(ReadButton() == SELECT) // debounce MENU button
{
// Stop motors before entering menu
MotorSpeed(LEFT_MOTOR, 0);
MotorSpeed(RIGHT_MOTOR, 0);
Clear();
Cursor(TOP, 0);
Print("Entering menu");
currentState = menu;
delay(1000);
lapTimer.disable(0);
return;
}
}
// Read raw sensor values
left = analogRead(LEFT_SENSOR);
right = analogRead(RIGHT_SENSOR);
leftDetected = left > thresholdLeft.Value;
rightDetected = right > thresholdRight.Value;
if (leftDetected || rightDetected)
{
CenterSmartUpdate();
}
// Display values on LCD
lcdRefreshCount = (lcdRefreshCount <= 0) ? lcdRefreshPeriod : (lcdRefreshCount - 1);
batteryCount = (batteryCount <= 0) ? batteryPeriod : (batteryCount - 1);
DisplaySensorValues();
}
int test02(void)
{
Init();
StatusLED(XGN);
FrontLED(ON);
BackLED(ON, ON);
MotorDir(FWD, RWD);
MotorSpeed(150,80);
while(1);
return 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;
}
void Kollision(void){
char switches = '0';
char zero = '0';
char one = '0';
char two = '0';
char three = '0';
char four = '0';
char five = '0';
switches = PollSwitch() & PollSwitch() & 0b00111111;
zero = switches & SWITCH(1);
one = switches & SWITCH(2);
two = switches & SWITCH(1);
three = switches & SWITCH(2);
four = switches & SWITCH(1);
five = switches & SWITCH(2);
if(zero || one || two || three || four || five){
MotorSpeed(0,0);
StereoSound(duck_melody,duck_melody);
Lichtorgel();
}
}
/* 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();
}
inline void CenterSmartUpdate()
{
center = analogRead(CENTER_SENSOR);
if (centerTimeoutCount)
centerTimeoutCount += 1;
if (!centerOldHigh && (center > centerSchmittHigh.Value)) // On switch-to-high
{
centerOldHigh = true; // Switch current center state to high
if((intersectionSignalRecent + 1) == INTERSECTION_COUNT_ORIGIN) // If the start/stop signal is encountered, toggle clock.
// This is done on when it switches high (instead of waiting for timeout) to save a bit of clock-time.
{
lapTimer.toggle(0);
if (!lapTimer.isEnabled(0))
{
// Stop motors before entering menu
MotorSpeed(LEFT_MOTOR, 0);
MotorSpeed(RIGHT_MOTOR, 0);
Clear();
Cursor(TOP, 0);
Print("Finish!");
currentState = menu;
delay(800);
return;
}
}
switch(intersectionTurnFlag)
{
case INTERSECTION_COUNT_LEFT: // First intersection after left signal
currentState = turnLeft;
break;
case INTERSECTION_COUNT_RIGHT: // First intersection after right signal
currentState = turnRight;
break;
default: // Else
currentState = moveStraight;
intersectionSignalRecent += 1;
break;
}
centerTimeoutCount = 0; // Stop timeout counter
intersectionTurnFlag = 0; // Reset flag
}
else if (centerOldHigh && (center < centerSchmittLow.Value)) // On switch-to-low
{
centerOldHigh = false; // Swtich current center state to low
centerTimeoutCount = 1; // Start timeout counter
}
if (centerTimeoutCount >= (centerTimeoutLimit.Value * 2)) // Timeout
{
if (intersectionTurnFlag < intersectionSignalRecent) // Do not let false signals effect the flag
intersectionTurnFlag = intersectionSignalRecent; // Set flag
intersectionSignalRecent = 0; // Reset signal counter
centerTimeoutCount = 0; // Stop timeout counter
}
}
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
}
/*!
\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);
}
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 ();
}
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;
}
void setMotorSpeed(unsigned char SpeedLeft, unsigned char SpeedRight){
IstSpeedLeft = constrain(IstSpeedLeft, 100, 255);
IstSpeedRight = constrain(IstSpeedRight, 100, 255);
MotorSpeed(SpeedLeft, SpeedRight);
}
int _tmain(int argc, _TCHAR* argv[])
{
/*------------------------Constants---------------------------------------------------------*/
CPipe motor("MotorData",1024);
CPipe motionComplete("Stopped",1024);
char* address1 = "0"; //corresponds to motor driver 1 on ST400
char* address2 = "3"; //motor driver 3
int slew_rate = 400; //This is the maximum step rate (range is 16 to 8500 steps/sec)
int first_rate = 100; //This is the initial step rate (range is 16 to 8500 steps/sec)
int accel = 1; //This is the acceleration rate (range is 0 to 255 steps/sec^2)
int current = 300; //Motor winding current in mAs
int port = 3; //Port number varies depending on the computer used.
//Check Device Manager for proper port assignment.
int RightWheelDone = 0; //Bit 7 set to 1 when motor movement is completed
int LeftWheelDone = 0;
int jogSpeed = 500; // Jog Speed (testing purposes only)
/*------------------------------------------------------------------------------------------*/
//Attempt to open the bluetooth port to begin sending commands.
while(PortOpen(port, 9600)!=1)
{
char* error = ::RMVGetErrorMessage();
printf("Port not opened. Error=%s\n",error);
Sleep(1000);
}
printf("Port successfully opened\n");
//Initialize the stepper motors
MotorStationsInit();
//Configure the motors to half-step mode
if(MotorConfigureMode(address1,1)!=1 || MotorConfigureMode(address2,1)!=1)
{
//An error has occured. Display the RMV error message to determine the error.
char* error = ::RMVGetErrorMessage();
printf("Configure error=%s\n",error);
}
//Initialize the slew rate, first rate and acceleration
if(MotorSpeed( address1, slew_rate, first_rate, accel )!=1 ||
MotorSpeed( address2, slew_rate, first_rate, accel )!=1)
{
//An error has occured. Display the RMV error message to determine the error.
char* error = ::RMVGetErrorMessage();
printf("Motor Speed error=%s\n",error);
}
if ( MotorSetCurrent(address1, current )!=1 || MotorSetCurrent(address2,current)!=1)
{
//An error has occured. Display the RMV error message to determine the error.
char* error = ::RMVGetErrorMessage();
printf("Set Current error = %s\n",error);
} else {
printf("\nMotor Current set to: %d",current);
}
//Main motor control loop. Read step instructions sent by Thursday and perform them.
while(1){
//Await instructions from Trajectory Planning program (Thursday).
motor.Read(&ST400,sizeof(ST400));
cout << "Steps (right, left) = "<< ST400.rightWheel << "\t"<<ST400.leftWheel << endl;
//Set the number of steps the motors must travel.
if(MotorNumberStepRel( address1 , -ST400.leftWheel)!=1 || MotorNumberStepRel( address2 , -ST400.rightWheel)!=1)
{
//An error has occured. Display the RMV error message to determine the error.
char* error = ::RMVGetErrorMessage();
printf("Step setting error=%s\n",error);
}
//Put motors in jog mode (testing purposes only) Comment out MotorNumberStepRel if using.
/*MotorJog ( address1, 1 );
MotorJog ( address2, 1 );
MotorChangeJogSpeed ( address1, jogSpeed);
MotorChangeJogSpeed ( address2, jogSpeed));*/
//Tell the motors to start executing the received command
if(MotorTriggerON(address1)!=1 || MotorTriggerON(address2)!=1)
{
//An error has occured. Display the RMV error message to determine the error.
char* error = ::RMVGetErrorMessage();
printf("Trigger on error=%s\n",error);
}
//Loop until current command is completed (bit 7 is set to 1) or an obstacle is detected
while(((RightWheelDone & 0x80) != 128) || ((LeftWheelDone & 0x80) != 128)){
//Read motor status register to determine if current motion has ended
if(MotorGetStatusRegister(address1,&LeftWheelDone)!= 1 ||
MotorGetStatusRegister(address2,&RightWheelDone)!=1)
{
//An error has occured. Display the RMV error message to determine the error.
char* error = ::RMVGetErrorMessage();
printf("Get Status error=%s\n",error);
}
//Check if Thursday has sent data and read it
if(motor.TestForData()>=sizeof(ST400))
{
motor.Read(&ST400,sizeof(ST400));
}
//Check if Thursday has indicated motor motion should stop (i.e. if an obstacle has been
//detected by the IR sensors) and end the current motor motion if it has.
if(ST400.stop != 0)
{
//Stop the current motion
if(MotorStop(address1)!=1 || MotorStop(address2)!=1)
{
//An error has occured. Display the RMV error message to determine the error.
char* error = ::RMVGetErrorMessage();
printf("Stop error=%s\n",error);
}
ST400.stop = 0;
//Exit nested while loop
break;
}
}
//Send a 1 to Thursday to indicate current instructions are completed.
int done=1;
motionComplete.Write(&done,sizeof(done));
//Reset done variables
LeftWheelDone = 0;
RightWheelDone = 0;
MotorTriggerOFF(address1);
MotorTriggerOFF(address2);
/*
if(MotorPowerOFF(address1)!=1 || MotorPowerOFF(address2)!=1)
{
//An error has occured. Display the RMV error message to determine the error.
char* error = ::RMVGetErrorMessage();
printf("error=%s\n",error);
}*/
}
}
int motor_init(){
MotorsInit();
MotorSpeed(LEFT_SIDE , 50<<8);
MotorSpeed(RIGHT_SIDE , 50<<8);
return 1;
}