Advertisement
Guest User

Untitled

a guest
Jul 17th, 2017
212
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #pragma config(Sensor, S1,     s_kompass,           sensorI2CHiTechnicCompass)
  2. #pragma config(Sensor, S2,     s_gyro,              sensorI2CHiTechnicGyro)
  3. #pragma config(Sensor, S4,     s_sonar,             sensorSONAR)
  4. #pragma config(Motor,  motorB,          B,             tmotorNormal, PIDControl, encoder)
  5. #pragma config(Motor,  motorC,          C,             tmotorNormal, PIDControl, encoder)
  6. //*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
  7.  
  8.  
  9.  
  10. /**********************************************************************************************************
  11. *                                                                                                         *
  12. *  Projekt: Lego Mindstorms Nxt - Segway                                                                  *
  13. *                                                                                                         *
  14. *  Studenten:                                                                                             *
  15. *  Steffen Burkhard                                                                                       *
  16. *  Stephan Machura                                                                                        *
  17. *                                                                                                         *
  18. *  Version: 3.0 - Init, readBtMsg, drive, CollisionDetection - task control, task main - home (PC-seitig) *
  19. *  -> Balancing-Algorithmus: OK; move forward, backward: OK; turn left, right: OK; Home: NOK              *
  20. ***********************************************************************************************************/
  21.  
  22.  
  23. /************************************************************************************************/
  24. // Includes
  25. // Include der BTconnection Datei
  26. #include "BTconnection.c"
  27. // Include fr Bt-Message Verarbeitung
  28. #include "BTmessage.c"
  29. /************************************************************************************************/
  30.  
  31.  
  32. /************************************************************************************************/
  33. // Definition globaler Variablen
  34. int gyrooffset = 600;//607; //0: // Variable fuer den GYRO_OFFSET
  35. int move=0; // Variable wird gesetzt, wenn Taste gedrueckt wurde
  36. int mv=0; // Variable fuer die Berechnung des Vorwaerts-oder Rueckwaerts fahrens
  37. int turn=0; // Variable wird gesetzt, wenn Taste gedrueckt wurde
  38. int trn=0; // Variable fuer die Berechnung des Rechts/Links fahrens
  39. int mov_diff=80; // Variable fuer move-Skalierung
  40. int trn_diff=6; //8; // Variable fuer turn-Skalierung
  41. bool detect=false; // Variable fuer CollisonDetection
  42. int IntervallCount = 40; //20; // Variable fuer die Anzahl der Balancing-Schleifendurchlaeufe bis control()-task gestartet wird
  43. /************************************************************************************************/
  44.  
  45.  
  46. /************************************************************************************************/
  47. // Funktionen
  48. //----------------------------------------------------------------------------------------------//
  49. // Initialisierungsfunktion <- Berechnung des GYRO_OFFSETs
  50. void init()
  51. {
  52.   /**
  53.     / Diese Funktion berechnet den gyrooffset, indem sie einen
  54.     / Mittelwert ueber einen festgelegte Anzahl (count=11) von
  55.     / Messwerten bildet.
  56.     **/
  57.  
  58.   // Initialisierung <- Berechnung des GYRO_OFFSETs
  59.     // Variablen
  60.     int sum=0, i=0;
  61.     int erg=0;
  62.     int count=11; // Anzahl der Messwerte die erhoben werden um den Gyro-Offset zu ermitteln
  63.  
  64.     // Display-Ausgabe
  65.     eraseDisplay();
  66.     nxtDisplayCenteredTextLine(0, "LegoSegway");
  67.     nxtDisplayCenteredTextLine(1, "Start init() !");
  68.  
  69.     // Bestimmung des GYRO_OFFSETs
  70.     for (i=0; i<count; i++)
  71.     {
  72.       sum=sum+SensorRaw(S2);
  73.     }
  74.     erg=sum/count;
  75.     gyrooffset=erg;
  76.     // nach Abschluss ist der Mittelwert aus 11 Gyro-Offset-Werten berechnet worden!
  77.     // Display-Ausgabe
  78.     nxtDisplayCenteredTextLine(2, "bere. Gyro: %d", gyrooffset);
  79.     nxtDisplayCenteredTextLine(3, "init Complete!");
  80. }
  81.  
  82. //----------------------------------------------------------------------------------------------//
  83. // Funktion zum Fahren
  84. void drive()
  85. {
  86.   /**
  87.   / Diese Funktion ist fuer die Steuerung des LegoSegways zustaendig.
  88.   / Die Funktion erhaelt die gedrueckte Taste von readBTMsg() und setzt die Variablen fuer die Bewegungen.
  89.   **/
  90.       // prueft auf BT-Verbindung
  91.       checkBTLinkConnected();
  92.  
  93.           //Display-Ausgabe
  94.         //eraseDisplay();
  95.         nxtDisplayCenteredTextLine(0, "LegoSegway");
  96.         nxtDisplayCenteredTextLine(1, "drive()!!!");
  97.  
  98.         //if(keyboard['W'])
  99.         if(keyboard[87]==true)
  100.         {
  101.               nxtDisplayCenteredTextLine(4, "--forward--");
  102.               turn=0;
  103.               //move=1;
  104.                 move=160;
  105.                 keyboard[87]=false;
  106.                 nxtDisplayCenteredTextLine(5, "mv  %d", mv);
  107.               nxtDisplayCenteredTextLine(6, "move %d", move);
  108.             }
  109.             //if(keyboard['A'])
  110.         else if(keyboard[65]==true)
  111.         {
  112.               nxtDisplayCenteredTextLine(4, "--left--");
  113.               turn=-24;
  114.               move=0;
  115.                 keyboard[65]=false;
  116.                 nxtDisplayCenteredTextLine(5, "mv  %d", mv);
  117.               nxtDisplayCenteredTextLine(6, "move %d", move);
  118.       }
  119.         //if(keyboard['S'])
  120.         else if(keyboard[83]==true)
  121.         {
  122.               nxtDisplayCenteredTextLine(4, "--backward--");
  123.               turn=0;
  124.               //move=-1;
  125.               move=-160;
  126.                 keyboard[83]=false;
  127.                 nxtDisplayCenteredTextLine(5, "mv  %d", mv);
  128.               nxtDisplayCenteredTextLine(6, "move %d", move);
  129.       }
  130.         //if(keyboard['D'])
  131.         else if(keyboard[68]==true)
  132.         {
  133.               nxtDisplayCenteredTextLine(4, "--right--");
  134.               turn=24;
  135.                 move=0;
  136.                 keyboard[68]=false;
  137.                 nxtDisplayCenteredTextLine(5, "mv  %d", mv);
  138.               nxtDisplayCenteredTextLine(6, "move %d", move);
  139.       }
  140.       //if(keyboard['H'])
  141.       else if(keyboard[72]==true)
  142.       {
  143.               nxtDisplayCenteredTextLine(4, "--home!!!--");
  144.               keyboard[72]=false;
  145.       }
  146.       else
  147.       {
  148.         // Move-Anteile und Turn-Anteile auf 0 setzen
  149.               mv = 0;
  150.                 move = 0;
  151.               turn = 0;
  152.               trn = 0;
  153.           }
  154. }
  155.  
  156. //----------------------------------------------------------------------------------------------//
  157. //Motoren-Korrekturfunktion
  158. void motorpower(int pw)
  159. {
  160.   /**
  161.     / Diese Funktion laesst die Motoren
  162.     / mit dem berechneten Korrekturfaktor als uebergebenen Power-Wert (pw)
  163.     / mit -100<=pw<=+100 rotieren. ein Move-Anteil wird in main() beim balancieren bereits,
  164.     / hinzugerechnet.
  165.     / Ein berechneter Turn-Anteil wird in dieser Funktion auf den uebergebenen Power-Wert hinzugerechnet.
  166.     **/
  167.  
  168.       motor[motorB]=pw-trn; // Motor right
  169.       motor[motorC]=pw+trn; // Motor left
  170. }
  171.  
  172. //----------------------------------------------------------------------------------------------//
  173. // Kollisionserkennungsfunktion
  174. void CollisionDetection()
  175. {
  176.     /**
  177.     /
  178.     / Diese Funktion spielt einen Ton ab,
  179.     / sobald die festgelegte Distanz (in cm) zu einem Hindernis unterschritten ist.
  180.     / Ausserdem wird das Vorwaerts bewegen des Segways verhindert.
  181.     /
  182.     **/
  183.  
  184.       // Variablen
  185.       int distance = 20; // festgelegter Abstand zu Hindernis
  186.       int currdist; // Variable fuer aktuellen Distanzsensorwert
  187.  
  188.         // Lese aktuellen Sensorwert (Distanz in cm) aus
  189.         currdist=SensorValue(S4);
  190.         // Kollisionsabfrage
  191.         if (currdist < distance)
  192.         {
  193.           detect = true;
  194.         PlaySound(soundBeepBeep);  // es wird ein Ton erzeugt
  195.         move=0;
  196.         //Display-Ausgabe
  197.             //eraseDisplay();
  198.           //nxtDisplayCenteredTextLine(0, "LegoSegway");
  199.           //nxtDisplayCenteredTextLine(1, "CollisonDet()");
  200.           nxtDisplayCenteredTextLine(5, "Stop Forward!");
  201.         }
  202.         detect=false;
  203. }
  204. /************************************************************************************************/
  205.  
  206.  
  207. /************************************************************************************************/
  208. //Tasks
  209. //----------------------------------------------------------------------------------------------//
  210. // control-Task
  211. task control()
  212. {
  213.   readBTMsg(); // prueft mailbox auf Messageeingang
  214.     drive(); // startet die Funktion fuer die Steuerung
  215.  
  216.     // Turn-Anteil berechnen
  217.     if(turn != 0)
  218.     {
  219.       trn = turn / trn_diff; // Turn-Anteil skalieren
  220.     }
  221.     return;
  222. }
  223.  
  224. //----------------------------------------------------------------------------------------------//
  225. // main()-Task
  226. task main()
  227. {
  228.   //Display-Ausgabe
  229.     eraseDisplay();
  230.   nxtDisplayCenteredTextLine(0, "LegoSegway");
  231.   nxtDisplayCenteredTextLine(1, "main()");
  232.   //nxtDisplayCenteredTextLine(3, "akt.Gyro:   %d", SensorRaw(S2));
  233.     //nxtDisplayCenteredTextLine(4, "Correction: %d", correction);
  234.  
  235.   // Variablendefinition
  236.     // Variablen fuer PD-Controller
  237.     int pvalgyro=12;
  238.     //int ivalgyro=1;
  239.     int dvalgyro=1;
  240.     int pvalwheel=5;
  241.     //int ivalwheel=0;
  242.     int dvalwheel=360;
  243.  
  244.     int angularrate; //Messwert der Winkelgeschwindigkeit <- proportionaler Anteil der Koerperwinkelgeschwindigkeit
  245.     int angularratesum=0; //Gesamtsumme der Koerperwinkelabweichung <- differentielle Anteil der Koerperwinkelgeschwindigkeit
  246.     //int angularrateint=0; //Mittelwert ueber alten und neuen Messwert <- integraler Anteil der Koerperwinkelgeschwindigkeit
  247.  
  248.     int wheelpos=0; //aktuelle Radwinkelstellung <- proportionaler Anteil der Radwinkelstellung
  249.     int wheelposdiff; //Differenz zwischen altem und neuen Messwert <- differentielle Anteil der Radwinkelstellung
  250.     //int wheelposint=0; //nicht verwendet !!! <- integraler Anteil der Radwinkelstellung
  251.     int wheelpospre=0; //vorherige Radwinkelstellung (fuer Berechnung notwendig)
  252.     int wheelpossum=0;
  253.  
  254.     int correction; //Variable fuer den Korrekturfaktor
  255.     int correctiondiv=5; // Skalierungsfaktor fuer Motoren-Power-Bereich zu erreichen (-100<=correction<=100)
  256.     int frictionregard=48; // Skalierungsfaktor fuer Beruecksichtigung des Reibungswiderstandes
  257.  
  258.     int limit=15; // Grenzwert für die Veraenderungsrate der Radwinkelstellungen
  259.     int timer=10;
  260.     int loops=0; //variable fuer Anzahl der durchgefuehrten Schleifen
  261.  
  262.     // Motor-Konfiguration
  263.     // Moeglichkeit fuer einen regulierten PID-control der Motoren
  264.     nMotorPIDSpeedCtrl[motorB] = mtrNoReg; // nicht regulierter Control
  265.     nMotorPIDSpeedCtrl[motorC] = mtrNoReg; // nicht regulierter Control
  266.     bFloatDuringInactiveMotorPWM=true;
  267.  
  268.     //BT im NXT aktivieren und sichtbar machen
  269.     if (bWaitForBluetoothIdle()) // Bluetooth ist in Betrieb
  270.     {
  271.     nxtDisplayCenteredTextLine(2, "BT = 'On'");
  272.     BluetoothOn();  // Aufruf der Funktion, die das Bluetooth-Modul eingeschaltet
  273.     nxtDisplayCenteredTextLine(3, "BT = Visible");
  274.     BluetoothVisible(true); // Aufruf der Funktion, die die Sichtbarkeit des Bluetoothes setzt
  275.   }
  276.  
  277.     //Aufruf der Initialisierungsfunktion
  278.     init();
  279.  
  280.     while(true)
  281.     {
  282.       // Balancierungsalgorithmus
  283.         // Winkelgeschwindigkeit aus Gyro-Sensor auslesen und Messwert durch abziehen des GYRO_OFFEST bereinigen
  284.         angularrate=SensorRaw(S2)-gyrooffset;
  285.         // der Messwert der Winkelgeschwindigkeit fliesst in Gesamtsumme der Koerperwinkelabweichung ein
  286.         angularratesum+=angularrate;
  287.         // vereinfacht: staendig berechneter Mittelwert ueber alten Mittelwert mit neuem Messwert der Winkelgeschwindigkeit
  288.         //angularrateint = (angularrateint + angularrate)/2;
  289.  
  290.         if(detect==true && move>0)
  291.         {
  292.           move=0;
  293.         }
  294.  
  295.         CollisionDetection(); //startet die Kollisionserkennungs-Funktion
  296.  
  297.         // Move Anteil skalieren
  298.         //mv = (move*100)/mov_diff;
  299.         mv = move / mov_diff;
  300.         // Radwinkelstellung aktueller Position auslesen
  301.         wheelpos=nMotorEncoder[motorB];
  302.         // hat sich Segway bewegt? -> Differenz zwischen neuem und alten Messwert bilden
  303.         wheelposdiff=wheelpos-wheelpospre;// wheelposdiff < 0 -> Segway nach hinten bewegt (rueckwaerts), wheelposdiff > 0 -> Segway nach vorne bewegt (vorwaerts)
  304.         // speichere aktuellen Messwert fuer zukuenftige Berechnung
  305.         wheelpospre=wheelpos;
  306.         // Berechnung der Veränderungsrate mit move-Anteil
  307.         wheelpossum+=wheelposdiff-mv;
  308.  
  309.         if (abs(wheelpossum)>limit)
  310.         {
  311.             /**
  312.             / Grenzwert der Veraenderungsrate ist nun ueberschritten
  313.             / GYRO_OFFSET in Abhaengigkeit des Vorzeichens veraendern, um ein Ueberschwingen zu verhindern muss der GYRO_OFFSET minimal veraendert werden
  314.             / Gyro-Offset +1/-1:
  315.             / wheel_pos_sum < 0 => GYRO_OFFSET -=-1 => GYRO_OFFSET+1
  316.             / wheel_pos_sum > 0 => GYRO_OFFSET -=1 => GYRO_OFFSET-1
  317.             / wheel_pos_sum = 0 => kann hier nicht vorkommen
  318.             **/
  319.             gyrooffset-=sgn(wheelpossum);
  320.             wheelpossum=0;
  321.         }
  322.  
  323.         /**
  324.         / Korrekturformel:
  325.         / correction = (PD-Regler_Winkel + PD-Regler_Raeder) * Skalierungsfaktor
  326.         / PID1 = p + i +d;
  327.         / p = k * e;          -> pvalgyro * angularrate
  328.         / i = k * (i-1 + e)   -> ivalgyro * angular_rate_int
  329.         / d = k * (e - (e-1)) -> dvalgyro * angularratesum
  330.     /
  331.         / PID2 = p + d;
  332.         / p = k * e;          -> pvalwheel * wheelpos
  333.         / i = k * (i-1 + e)   -> ivalwheel=0 !!!+
  334.         / d = k * (e - (e-1)) -> dvalwheel * wheelposdiff
  335.         **/
  336.  
  337.         //correction=(angularrate*pvalgyro+angularrateint*ivalgyro+angularratesum*dvalgyro+wheelpos*pvalwheel+wheelposdiff*dvalwheel)>>correctiondiv; //Controller
  338.  
  339.         correction=(angularrate*pvalgyro+angularratesum*dvalgyro+wheelpos*pvalwheel+wheelposdiff*dvalwheel)>>correctiondiv; //Controller
  340.  
  341.         /** Korrekturfaktor in Abhaengigkeit vom Vorzeichen bzgl. des Reibungswiderstandes anpassen
  342.         / correction > 0 => correction+=1*frictionregard
  343.         / correction < 0 => correction+=-1*frictionregard
  344.         / correction = 0 => correction+=0
  345.         **/
  346.         correction+=sgn(correction)*frictionregard;
  347.  
  348.         // steure der Bewegung entgegen, indem die Motoren mit correction als Power-Wert drehen
  349.         motorpower(correction);
  350.  
  351.         // nach festgelegter Anzahl von Schleifendurchgaengen werden einige Funktionen aufgerufen
  352.     if (loops==IntervallCount)
  353.         {
  354.           // die festgelegte Anzahl von Schleifendurchgaengen ist erreicht!
  355.       StartTask(control, 10); // control() --> readBTMsg(); drive();
  356.  
  357.             loops=0; //Schleifenanzahl zuruecksetzen
  358.             //timer=10;
  359.         }
  360.         // Schleifenanzahl erhoehen
  361.         loops+=1;
  362.         // Timer
  363.  
  364.         wait1Msec(timer);
  365.     }
  366. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement