Helvijs Kiselis
/
MicromousePES2
main.cpp: Sensoren einlesen und Motoren ansteuern
main.cpp
- Committer:
- luethale
- Date:
- 2018-04-16
- Revision:
- 8:862bf9225953
- Parent:
- 7:8d60a2ff4c2b
File content as of revision 8:862bf9225953:
#include <mbed.h> #include "EncoderCounter.h" #include "Controller.h" #include "IRSensor.h" #include "Motion.h" //User Button InterruptIn button(USER_BUTTON); //ciaone //Sensors: AnalogIn lineSensor(PA_4); AnalogIn distance2(PC_3); AnalogIn distance4(PB_1); AnalogIn distance1(PC_2); AnalogIn distance3(PC_5); IRSensor irSensorL (distance2); IRSensor irSensorC (distance4); IRSensor irSensorR (distance1); IRSensor irSensorB (distance3); //Motors: DigitalOut myled(LED1); DigitalOut enableMotorDriver(PB_2); DigitalIn motorDriverFault(PB_14); DigitalIn motorDriverWarning(PB_15); PwmOut pwmRight(PA_8); PwmOut pwmLeft(PA_10); EncoderCounter counterRight(PB_6, PB_7); EncoderCounter counterLeft(PA_0, PA_1); Controller controller(pwmLeft, pwmRight, counterLeft, counterRight); Motion motion(controller, counterLeft, counterRight, irSensorL, irSensorC, irSensorR, enableMotorDriver); //------------------------------------------------------------------------------ volatile int start = 0; short countsLeft = 0; short countsRight = 0; short countsLeftOld = 0; short countsRightOld = 0; //den jeweiligen Test auf 1 setzen int testSensor = 0; int testMotion = 0; int testRotation = 0; int testCounts = 0; int testClass = 0; //------------------------------------------------------------------------------ //User button toggle void press() { start = !start; } //------------------------------------------------------------------------------ int main() { /** Micromouse Test */ while(1) { button.fall(&press); //User button einlesen while(0) { myled = 1; printf("Hai\n"); if (start == 0) break; } /* Sensoren Test */ while(1) { float distanceL = irSensorL.readL(); float distanceC = irSensorC.readC(); float distanceR = irSensorR.readR(); float distanceB = irSensorB.readC(); printf("Links: %f Mitte: %f Rechts: %f Hinten: %f Line: %f\n", distanceL, distanceC, distanceR, distanceB, lineSensor.read()); //wait(0.5f); } /* Motoren Test: Gerade Bewegung */ while(start == 1 && testMotion == 1) { controller.setDesiredSpeedLeft(20.0f); //Drehzahl in [rpm] controller.setDesiredSpeedRight(-20.0f); enableMotorDriver = 1; //Schaltet den Leistungstreiber ein countsLeft = counterLeft.read(); countsRight = counterRight.read(); printf("countsLeft: %hd countsRight: %hd\n", countsLeft, countsRight); if (start == 0) { motion.stop(); } } /* Motoren Test: 180° Rotation */ while(start == 1 && testRotation == 1) { while((countsLeft - countsLeftOld) > -1614 || (countsRight - countsRightOld) > -1614) { countsLeft = counterLeft.read(); countsRight = counterRight.read(); controller.setDesiredSpeedLeft(-50.0f); controller.setDesiredSpeedRight(-50.0f); enableMotorDriver = 1; } motion.stop(); enableMotorDriver = 0; printf("countsLeft: %hd countsRight: %hd\n", countsLeft, countsRight); if (start == 0) { countsLeftOld = countsLeft - 100; countsRightOld = countsRight - 100; break; } } /* Benötigte Ticks für 180* Rotation am Ort: mm/count = 0.122 Dr = 124mm Rr = 60mm 180°=> 1597 counts 90°=> 798 counts RotationsDistanz Rd= DistanzRäder*pi/2 = 194.8 UmfangRad = Drad*pi = 188.5 Counts = Rd/mm pro Count => Counts pro Drehung rausfinden */ while(start == 1 && testCounts == 1) { while(countsLeft < 5000 || countsRight > -5000) { countsLeft = counterLeft.read(); countsRight = counterRight.read(); controller.setDesiredSpeedLeft(20.0f); controller.setDesiredSpeedRight(-20.0f); enableMotorDriver = 1; } controller.setDesiredSpeedLeft(0.0f); controller.setDesiredSpeedRight(0.0f); enableMotorDriver = 0; } //Class test if(start == 1 && testClass == 1) { motion.rotate180(); counterLeft.reset(); counterRight.reset(); start = 0; } //Sensor calib if(start == 1) { int i; myled = !myled; for (i=0;i<5;i++) { float distanceL = irSensorL.readL(); float distanceC = irSensorC.readC(); float distanceR = irSensorR.readR(); float distanceB = irSensorB.readC(); printf("Links: %f Mitte: %f Rechts: %f Hinten: %f Line: %f\n", distanceL, distanceC, distanceR, distanceB, lineSensor.read()); } printf("------------------------\n"); start = 0; /* while((countsLeft - countsLeftOld) > -82 || (countsRight - countsRightOld) < 82) { controller.setDesiredSpeedLeft(-10.0f); //Drehzahl in [rpm] controller.setDesiredSpeedRight(10.0f); enableMotorDriver = 1; //Schaltet den Leistungstreiber ein countsLeft = counterLeft.read(); countsRight = counterRight.read(); } controller.setDesiredSpeedLeft(0.0f); controller.setDesiredSpeedRight(0.0f); enableMotorDriver = 0; float distanceL = irSensorB.read(); printf("Hinten: %f\n", distanceL); countsLeftOld = countsLeft; countsRightOld = countsRight; start = 0;*/ } } }