MotorDriver.cpp

00001 #include "MotorDriver.h"
00002 #include "Controller.h"
00003 #include "ReadSensor.h"
00004 #include "mbed.h"
00005 
00006 DigitalOut enableMotorDriver(PB_2);
00007 DigitalIn motorDriverFault(PB_14);
00008 DigitalIn motorDriverWarning(PB_15);
00009 
00010 PwmOut pwmLeft(PA_8);
00011 PwmOut pwmRight(PA_9);
00012 
00013 EncoderCounter counterLeft(PB_6, PB_7);
00014 EncoderCounter counterRight(PA_6, PC_7);
00015 
00016 DigitalOut myled(LED2);
00017 
00018 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
00019 
00020 void resetCounter(void);
00021 
00022 void startup (void)
00023 {
00024     enableMotorDriver = 1; //Schaltet den Motortreiber ein
00025     counterLeft.reset();   //Counter reseten
00026     counterRight.reset();
00027 }
00028 
00029 void driveOne(int NumbField, int SpeedMode)
00030 
00031 {
00032     int AnzahlZyklen = 0;
00033     float distance_side;
00034     float distance_front;
00035     float CorrectFactor;
00036     float Speed = 0.0f;
00037 
00038     switch(SpeedMode) {         //Geschwindigkeit einstellen
00039         case 1:
00040             Speed = 20.0f;
00041             break;
00042 
00043 
00044         case 2:
00045             Speed = 40.0f;
00046             break;
00047 
00048         case 3:
00049             Speed = 60.0f;
00050             break;
00051 
00052 
00053         default:
00054 
00055             break;
00056     }
00057 
00058     //Fährt bis Anzahl umdrehungen erreicht sind
00059     while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 990*NumbField) {
00060         distance_front = readSensorValue(2);    //Distanz zur vorderen Wand wird gemessen
00061         if(readSensorValue(1) < 15.0f)
00062         {
00063         distance_side= readSensorValue(1);      //Distanz zur linken Wand wird gemessen
00064         AnzahlZyklen = AnzahlZyklen+1;      //Anzahl Regelvorgänge werden gezählt
00065         distance_side = distance_side - 6.9f;         //Distanz zwischen Position und Ideallinie wird berechnet
00066         CorrectFactor = distance_side*2.0f;      //P Faktor des P-Reglers
00067         }
00068         else
00069         {
00070         distance_side= readSensorValue(3);      //Distanz zur rechten Wand wird gemessen
00071         AnzahlZyklen = AnzahlZyklen+1;      //Anzahl Regelvorgänge werden gezählt
00072         distance_side = distance_side - 6.9f;         //Distanz zwischen Position und Ideallinie wird berechnet
00073         CorrectFactor = distance_side*-2.0f;      //P Faktor des P-Reglers
00074         }
00075 
00076 
00077         //printf("%f\n",CorrectFactor);
00078         if(abs(CorrectFactor) <= 16.0f) {    //erkennt Wand oder Lücke auf linker Seite
00079             controller.setDesiredSpeedLeft((Speed) - CorrectFactor); //Bei Wand wird korrigiert
00080             controller.setDesiredSpeedRight((Speed * (-1.0f)) - CorrectFactor);
00081         } else {
00082             controller.setDesiredSpeedRight(Speed * (-1.0f));                 //Bei Lücke wird nach Counter gefahren
00083             controller.setDesiredSpeedLeft(Speed);
00084         }
00085         
00086         if(distance_front <= 3)
00087         {
00088             break;
00089         }
00090         
00091         
00092     }
00093     stop();
00094 
00095 }
00096 //Rechts abbiegen
00097 int turnRight(int direction)                
00098 {
00099     controller.setDesiredSpeedLeft(40.0f); // Drehzahl in [rpm] bei 31/1 enspricht es ca. 374/min
00100     controller.setDesiredSpeedRight(40.0f);
00101     while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 580) {
00102         //do nothing
00103     }
00104     stop();
00105     direction = direction + 1;
00106     if(direction == 5)
00107     {
00108         direction = 1;
00109     }
00110     return direction;
00111 }
00112 //links abbigen
00113 int turnLeft(int direction)
00114 {
00115     controller.setDesiredSpeedRight(-40.0f); // Drehzahl in [rpm] bei 31/1 enspricht es ca. 374/min
00116     controller.setDesiredSpeedLeft(-40.0f);
00117     while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 580) {
00118         //do nothing
00119     }
00120     stop();
00121     direction = direction - 1;
00122         if(direction == 0)
00123     {
00124         direction = 4;
00125     }
00126     return direction;
00127 }
00128 
00129 //Roboter anhalten
00130 void stop(void)
00131 {
00132     controller.setDesiredSpeedRight(0.0f);
00133     controller.setDesiredSpeedLeft(0.0f);
00134     resetCounter();
00135 
00136 }
00137 
00138 void resetCounter(void)
00139 {
00140     controller.DetachTicker();
00141     counterLeft.reset();
00142     counterRight.reset();
00143     controller.setDesiredSpeedRight(0.0f);
00144     controller.setDesiredSpeedLeft(0.0f);
00145     controller.AttachTicker();
00146 
00147 }
00148 
00149 void driveDist(int Distance, int Direction)
00150 {
00151     controller.setDesiredSpeedRight(-20.0f*Direction); // Drehzahl in [rpm]
00152     controller.setDesiredSpeedLeft(20.0f*Direction);
00153 
00154     while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 100*Distance) {
00155         //Fährt bis Anzahl umdrehungen erreicht sind
00156     }
00157     stop();
00158 }
00159 
00160