Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #pragma config(Sensor, S1, s_kompass, sensorI2CHiTechnicCompass)
- #pragma config(Sensor, S2, s_gyro, sensorI2CHiTechnicGyro)
- #pragma config(Sensor, S4, s_sonar, sensorSONAR)
- #pragma config(Motor, motorB, B, tmotorNormal, PIDControl, encoder)
- #pragma config(Motor, motorC, C, tmotorNormal, PIDControl, encoder)
- //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
- /**********************************************************************************************************
- * *
- * Projekt: Lego Mindstorms Nxt - Segway *
- * *
- * Studenten: *
- * Steffen Burkhard *
- * Stephan Machura *
- * *
- * Version: 3.0 - Init, readBtMsg, drive, CollisionDetection - task control, task main - home (PC-seitig) *
- * -> Balancing-Algorithmus: OK; move forward, backward: OK; turn left, right: OK; Home: NOK *
- ***********************************************************************************************************/
- /************************************************************************************************/
- // Includes
- // Include der BTconnection Datei
- #include "BTconnection.c"
- // Include fr Bt-Message Verarbeitung
- #include "BTmessage.c"
- /************************************************************************************************/
- /************************************************************************************************/
- // Definition globaler Variablen
- int gyrooffset = 600;//607; //0: // Variable fuer den GYRO_OFFSET
- int move=0; // Variable wird gesetzt, wenn Taste gedrueckt wurde
- int mv=0; // Variable fuer die Berechnung des Vorwaerts-oder Rueckwaerts fahrens
- int turn=0; // Variable wird gesetzt, wenn Taste gedrueckt wurde
- int trn=0; // Variable fuer die Berechnung des Rechts/Links fahrens
- int mov_diff=80; // Variable fuer move-Skalierung
- int trn_diff=6; //8; // Variable fuer turn-Skalierung
- bool detect=false; // Variable fuer CollisonDetection
- int IntervallCount = 40; //20; // Variable fuer die Anzahl der Balancing-Schleifendurchlaeufe bis control()-task gestartet wird
- /************************************************************************************************/
- /************************************************************************************************/
- // Funktionen
- //----------------------------------------------------------------------------------------------//
- // Initialisierungsfunktion <- Berechnung des GYRO_OFFSETs
- void init()
- {
- /**
- / Diese Funktion berechnet den gyrooffset, indem sie einen
- / Mittelwert ueber einen festgelegte Anzahl (count=11) von
- / Messwerten bildet.
- **/
- // Initialisierung <- Berechnung des GYRO_OFFSETs
- // Variablen
- int sum=0, i=0;
- int erg=0;
- int count=11; // Anzahl der Messwerte die erhoben werden um den Gyro-Offset zu ermitteln
- // Display-Ausgabe
- eraseDisplay();
- nxtDisplayCenteredTextLine(0, "LegoSegway");
- nxtDisplayCenteredTextLine(1, "Start init() !");
- // Bestimmung des GYRO_OFFSETs
- for (i=0; i<count; i++)
- {
- sum=sum+SensorRaw(S2);
- }
- erg=sum/count;
- gyrooffset=erg;
- // nach Abschluss ist der Mittelwert aus 11 Gyro-Offset-Werten berechnet worden!
- // Display-Ausgabe
- nxtDisplayCenteredTextLine(2, "bere. Gyro: %d", gyrooffset);
- nxtDisplayCenteredTextLine(3, "init Complete!");
- }
- //----------------------------------------------------------------------------------------------//
- // Funktion zum Fahren
- void drive()
- {
- /**
- / Diese Funktion ist fuer die Steuerung des LegoSegways zustaendig.
- / Die Funktion erhaelt die gedrueckte Taste von readBTMsg() und setzt die Variablen fuer die Bewegungen.
- **/
- // prueft auf BT-Verbindung
- checkBTLinkConnected();
- //Display-Ausgabe
- //eraseDisplay();
- nxtDisplayCenteredTextLine(0, "LegoSegway");
- nxtDisplayCenteredTextLine(1, "drive()!!!");
- //if(keyboard['W'])
- if(keyboard[87]==true)
- {
- nxtDisplayCenteredTextLine(4, "--forward--");
- turn=0;
- //move=1;
- move=160;
- keyboard[87]=false;
- nxtDisplayCenteredTextLine(5, "mv %d", mv);
- nxtDisplayCenteredTextLine(6, "move %d", move);
- }
- //if(keyboard['A'])
- else if(keyboard[65]==true)
- {
- nxtDisplayCenteredTextLine(4, "--left--");
- turn=-24;
- move=0;
- keyboard[65]=false;
- nxtDisplayCenteredTextLine(5, "mv %d", mv);
- nxtDisplayCenteredTextLine(6, "move %d", move);
- }
- //if(keyboard['S'])
- else if(keyboard[83]==true)
- {
- nxtDisplayCenteredTextLine(4, "--backward--");
- turn=0;
- //move=-1;
- move=-160;
- keyboard[83]=false;
- nxtDisplayCenteredTextLine(5, "mv %d", mv);
- nxtDisplayCenteredTextLine(6, "move %d", move);
- }
- //if(keyboard['D'])
- else if(keyboard[68]==true)
- {
- nxtDisplayCenteredTextLine(4, "--right--");
- turn=24;
- move=0;
- keyboard[68]=false;
- nxtDisplayCenteredTextLine(5, "mv %d", mv);
- nxtDisplayCenteredTextLine(6, "move %d", move);
- }
- //if(keyboard['H'])
- else if(keyboard[72]==true)
- {
- nxtDisplayCenteredTextLine(4, "--home!!!--");
- keyboard[72]=false;
- }
- else
- {
- // Move-Anteile und Turn-Anteile auf 0 setzen
- mv = 0;
- move = 0;
- turn = 0;
- trn = 0;
- }
- }
- //----------------------------------------------------------------------------------------------//
- //Motoren-Korrekturfunktion
- void motorpower(int pw)
- {
- /**
- / Diese Funktion laesst die Motoren
- / mit dem berechneten Korrekturfaktor als uebergebenen Power-Wert (pw)
- / mit -100<=pw<=+100 rotieren. ein Move-Anteil wird in main() beim balancieren bereits,
- / hinzugerechnet.
- / Ein berechneter Turn-Anteil wird in dieser Funktion auf den uebergebenen Power-Wert hinzugerechnet.
- **/
- motor[motorB]=pw-trn; // Motor right
- motor[motorC]=pw+trn; // Motor left
- }
- //----------------------------------------------------------------------------------------------//
- // Kollisionserkennungsfunktion
- void CollisionDetection()
- {
- /**
- /
- / Diese Funktion spielt einen Ton ab,
- / sobald die festgelegte Distanz (in cm) zu einem Hindernis unterschritten ist.
- / Ausserdem wird das Vorwaerts bewegen des Segways verhindert.
- /
- **/
- // Variablen
- int distance = 20; // festgelegter Abstand zu Hindernis
- int currdist; // Variable fuer aktuellen Distanzsensorwert
- // Lese aktuellen Sensorwert (Distanz in cm) aus
- currdist=SensorValue(S4);
- // Kollisionsabfrage
- if (currdist < distance)
- {
- detect = true;
- PlaySound(soundBeepBeep); // es wird ein Ton erzeugt
- move=0;
- //Display-Ausgabe
- //eraseDisplay();
- //nxtDisplayCenteredTextLine(0, "LegoSegway");
- //nxtDisplayCenteredTextLine(1, "CollisonDet()");
- nxtDisplayCenteredTextLine(5, "Stop Forward!");
- }
- detect=false;
- }
- /************************************************************************************************/
- /************************************************************************************************/
- //Tasks
- //----------------------------------------------------------------------------------------------//
- // control-Task
- task control()
- {
- readBTMsg(); // prueft mailbox auf Messageeingang
- drive(); // startet die Funktion fuer die Steuerung
- // Turn-Anteil berechnen
- if(turn != 0)
- {
- trn = turn / trn_diff; // Turn-Anteil skalieren
- }
- return;
- }
- //----------------------------------------------------------------------------------------------//
- // main()-Task
- task main()
- {
- //Display-Ausgabe
- eraseDisplay();
- nxtDisplayCenteredTextLine(0, "LegoSegway");
- nxtDisplayCenteredTextLine(1, "main()");
- //nxtDisplayCenteredTextLine(3, "akt.Gyro: %d", SensorRaw(S2));
- //nxtDisplayCenteredTextLine(4, "Correction: %d", correction);
- // Variablendefinition
- // Variablen fuer PD-Controller
- int pvalgyro=12;
- //int ivalgyro=1;
- int dvalgyro=1;
- int pvalwheel=5;
- //int ivalwheel=0;
- int dvalwheel=360;
- int angularrate; //Messwert der Winkelgeschwindigkeit <- proportionaler Anteil der Koerperwinkelgeschwindigkeit
- int angularratesum=0; //Gesamtsumme der Koerperwinkelabweichung <- differentielle Anteil der Koerperwinkelgeschwindigkeit
- //int angularrateint=0; //Mittelwert ueber alten und neuen Messwert <- integraler Anteil der Koerperwinkelgeschwindigkeit
- int wheelpos=0; //aktuelle Radwinkelstellung <- proportionaler Anteil der Radwinkelstellung
- int wheelposdiff; //Differenz zwischen altem und neuen Messwert <- differentielle Anteil der Radwinkelstellung
- //int wheelposint=0; //nicht verwendet !!! <- integraler Anteil der Radwinkelstellung
- int wheelpospre=0; //vorherige Radwinkelstellung (fuer Berechnung notwendig)
- int wheelpossum=0;
- int correction; //Variable fuer den Korrekturfaktor
- int correctiondiv=5; // Skalierungsfaktor fuer Motoren-Power-Bereich zu erreichen (-100<=correction<=100)
- int frictionregard=48; // Skalierungsfaktor fuer Beruecksichtigung des Reibungswiderstandes
- int limit=15; // Grenzwert für die Veraenderungsrate der Radwinkelstellungen
- int timer=10;
- int loops=0; //variable fuer Anzahl der durchgefuehrten Schleifen
- // Motor-Konfiguration
- // Moeglichkeit fuer einen regulierten PID-control der Motoren
- nMotorPIDSpeedCtrl[motorB] = mtrNoReg; // nicht regulierter Control
- nMotorPIDSpeedCtrl[motorC] = mtrNoReg; // nicht regulierter Control
- bFloatDuringInactiveMotorPWM=true;
- //BT im NXT aktivieren und sichtbar machen
- if (bWaitForBluetoothIdle()) // Bluetooth ist in Betrieb
- {
- nxtDisplayCenteredTextLine(2, "BT = 'On'");
- BluetoothOn(); // Aufruf der Funktion, die das Bluetooth-Modul eingeschaltet
- nxtDisplayCenteredTextLine(3, "BT = Visible");
- BluetoothVisible(true); // Aufruf der Funktion, die die Sichtbarkeit des Bluetoothes setzt
- }
- //Aufruf der Initialisierungsfunktion
- init();
- while(true)
- {
- // Balancierungsalgorithmus
- // Winkelgeschwindigkeit aus Gyro-Sensor auslesen und Messwert durch abziehen des GYRO_OFFEST bereinigen
- angularrate=SensorRaw(S2)-gyrooffset;
- // der Messwert der Winkelgeschwindigkeit fliesst in Gesamtsumme der Koerperwinkelabweichung ein
- angularratesum+=angularrate;
- // vereinfacht: staendig berechneter Mittelwert ueber alten Mittelwert mit neuem Messwert der Winkelgeschwindigkeit
- //angularrateint = (angularrateint + angularrate)/2;
- if(detect==true && move>0)
- {
- move=0;
- }
- CollisionDetection(); //startet die Kollisionserkennungs-Funktion
- // Move Anteil skalieren
- //mv = (move*100)/mov_diff;
- mv = move / mov_diff;
- // Radwinkelstellung aktueller Position auslesen
- wheelpos=nMotorEncoder[motorB];
- // hat sich Segway bewegt? -> Differenz zwischen neuem und alten Messwert bilden
- wheelposdiff=wheelpos-wheelpospre;// wheelposdiff < 0 -> Segway nach hinten bewegt (rueckwaerts), wheelposdiff > 0 -> Segway nach vorne bewegt (vorwaerts)
- // speichere aktuellen Messwert fuer zukuenftige Berechnung
- wheelpospre=wheelpos;
- // Berechnung der Veränderungsrate mit move-Anteil
- wheelpossum+=wheelposdiff-mv;
- if (abs(wheelpossum)>limit)
- {
- /**
- / Grenzwert der Veraenderungsrate ist nun ueberschritten
- / GYRO_OFFSET in Abhaengigkeit des Vorzeichens veraendern, um ein Ueberschwingen zu verhindern muss der GYRO_OFFSET minimal veraendert werden
- / Gyro-Offset +1/-1:
- / wheel_pos_sum < 0 => GYRO_OFFSET -=-1 => GYRO_OFFSET+1
- / wheel_pos_sum > 0 => GYRO_OFFSET -=1 => GYRO_OFFSET-1
- / wheel_pos_sum = 0 => kann hier nicht vorkommen
- **/
- gyrooffset-=sgn(wheelpossum);
- wheelpossum=0;
- }
- /**
- / Korrekturformel:
- / correction = (PD-Regler_Winkel + PD-Regler_Raeder) * Skalierungsfaktor
- / PID1 = p + i +d;
- / p = k * e; -> pvalgyro * angularrate
- / i = k * (i-1 + e) -> ivalgyro * angular_rate_int
- / d = k * (e - (e-1)) -> dvalgyro * angularratesum
- /
- / PID2 = p + d;
- / p = k * e; -> pvalwheel * wheelpos
- / i = k * (i-1 + e) -> ivalwheel=0 !!!+
- / d = k * (e - (e-1)) -> dvalwheel * wheelposdiff
- **/
- //correction=(angularrate*pvalgyro+angularrateint*ivalgyro+angularratesum*dvalgyro+wheelpos*pvalwheel+wheelposdiff*dvalwheel)>>correctiondiv; //Controller
- correction=(angularrate*pvalgyro+angularratesum*dvalgyro+wheelpos*pvalwheel+wheelposdiff*dvalwheel)>>correctiondiv; //Controller
- /** Korrekturfaktor in Abhaengigkeit vom Vorzeichen bzgl. des Reibungswiderstandes anpassen
- / correction > 0 => correction+=1*frictionregard
- / correction < 0 => correction+=-1*frictionregard
- / correction = 0 => correction+=0
- **/
- correction+=sgn(correction)*frictionregard;
- // steure der Bewegung entgegen, indem die Motoren mit correction als Power-Wert drehen
- motorpower(correction);
- // nach festgelegter Anzahl von Schleifendurchgaengen werden einige Funktionen aufgerufen
- if (loops==IntervallCount)
- {
- // die festgelegte Anzahl von Schleifendurchgaengen ist erreicht!
- StartTask(control, 10); // control() --> readBTMsg(); drive();
- loops=0; //Schleifenanzahl zuruecksetzen
- //timer=10;
- }
- // Schleifenanzahl erhoehen
- loops+=1;
- // Timer
- wait1Msec(timer);
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement