Diff: main.cpp
- Revision:
- 0:8491169be8fc
- Child:
- 1:2b5f79285a3e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Apr 16 10:27:10 2018 +0000
@@ -0,0 +1,235 @@
+#include <mbed.h>
+
+#include "EncoderCounter.h"
+#include "Controller.h"
+#include "IRSensor.h"
+#include "Motion.h"
+
+//User Button
+ InterruptIn button(USER_BUTTON);
+
+//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;*/
+
+ }
+}
+}
+
+
+
+
\ No newline at end of file
Helvijs Kiselis
