Jan Mader
Distanzhelm
Diff: main.cpp
- Revision:
- 6:9b34fadd6628
- Parent:
- 4:67506e285ad0
- Child:
- 7:3b3a2c158803
--- a/main.cpp Fri Apr 02 09:33:18 2021 +0200
+++ b/main.cpp Mon Apr 26 08:33:06 2021 +0000
@@ -6,123 +6,219 @@
#include "Servo.h"
#include "SpeedController.h"
-int main()
-{
-
+using namespace std::chrono;
+
+
DigitalIn user_button(USER_BUTTON);
-
- // initialise PWM
- PwmOut pwm_motor_0(PB_13);
- PwmOut pwm_motor_1(PA_9);
- PwmOut pwm_motor_2(PA_10);
-
- // crete Encoder read objects
- EncoderCounter encoderCounter_0(PA_6, PC_7);
- EncoderCounter encoderCounter_1(PB_6, PB_7);
- EncoderCounter encoderCounter_2(PA_0, PA_1);
-
- // create controller
- SpeedController speedController_0(pwm_motor_0, encoderCounter_0);
- SpeedController speedController_1(pwm_motor_1, encoderCounter_1);
-
- DigitalOut enable_dc_motors(PB_15);
+ DigitalOut led(LED1);
// create servo objects
- Servo servo_0(PB_2);
- Servo servo_1(PC_8);
- Servo servo_2(PC_6);
+ Servo servo_0(PA_6);
- SDBlockDevice sd(PC_12, PC_11, PC_10, PD_2);
- printf("BlockDevice created\r\n");
- FATFileSystem fs("fs", &sd);
-
- // Initialise the digital pin LED1 as an output
- DigitalOut myled(LED1);
- // initialise PWM
- pwm_motor_0.period(0.00005f);// 0.05ms 20KHz
- pwm_motor_0.write(0.5f);
- pwm_motor_1.period(0.00005f);// 0.05ms 20KHz
- pwm_motor_1.write(0.5f);
- pwm_motor_2.period(0.00005f);// 0.05ms 20KHz
- pwm_motor_2.write(0.5f);
+ //Input
+ DigitalIn pirrechts(PB_12);
+ DigitalIn pirlinks(PC_7); //pc_6 puuty besetzt
+ DigitalIn pirvorne(PB_7); //pc_8
+ DigitalIn pirhinten(PA_1); //pc-9
+ DigitalIn ultrarechts(PB_1);
+ DigitalIn ultralinks(PC_5); //immer eins
+ DigitalIn ultravorne(PC_3);
+ DigitalIn ultrahinten(PC_2);// immer eins
- // initialise and test Servo
- servo_0.Enable(1000, 20000); // 1 ms / 20 ms
- servo_1.Enable(1000, 20000);
- int servo_desval_0 = 0;
- int servo_desval_1 = 0;
+ //Output
+ //Servo
+ DigitalOut pumpe(PB_6);
+ DigitalOut summer(PA_0);
+
/* input your stuff here */
- AnalogIn analogIn(PC_2);
- float dist = 0.0f;
+
+Timer loop_timer;
+int Ts_ms = 500;
+char CZustand = 55;
+int spritz = 3000; //spritzzeit
+
+
+
+/*Interrupt*/
+
+/*
+class Counter {
+public:
+ Counter(PinName pin) : _interrupt(pin) { // create the InterruptIn on the pin specified to Counter
+ _interrupt.rise(this, &Counter::increment); // attach increment function of this counter instance
+ }
+
+ void increment() {
+ _count++;
+ }
+
+ int read() {
+ return _count;
+ }
+
+private:
+ InterruptIn _interrupt;
+ volatile int _count;
+};
+
+*/
+
+
+int main()
+{
+
+ servo_0.Enable(1500,20000);
+
- /*
- printf("Test writing... ");
- FILE* fp = fopen("/fs/data.csv", "w");
- fprintf(fp, "test %.5f\r\n",1.23);
- fclose(fp);
- printf("done\r\n");
-
- printf("Test reading... ");
- // read from SD card
- fp = fopen("/fs/data.csv", "r");
- if (fp != NULL) {
- char c = fgetc(fp);
- if (c == 't')
- printf("done\r\n");
- else
- printf("incorrect char (%c)!\n", c);
- fclose(fp);
- } else {
- printf("Reading failed!\n");
- }
- */
-
- // enable driver DC motors
- enable_dc_motors = 1;
+
while (true) {
+
+ loop_timer.reset();
+ if(user_button == 0)
+ {
+ printf("nicht aktiv");
+ CZustand = 0;
+
+ }
+
+ /* ------------- start hacking ------------- -------------*/
+ if(CZustand == 0) // Aufstarten
+ {
+ //servo_0.SetPosition(1000);
+ printf("Grundposition\n");
- if(!user_button) {
- // LED off, set speedController speed, pwm2, position servo
- myled = 0;
-
- speedController_0.setDesiredSpeed( 60.0f);
- speedController_1.setDesiredSpeed(-120.0f);
- pwm_motor_2.write(0.7f);
-
- servo_0.SetPosition(servo_desval_0);
- servo_1.SetPosition(servo_desval_1);
- if(servo_desval_0 < 10000) servo_desval_0 += 100;
- if(servo_desval_1 < 10000) servo_desval_1 += 100;
+ thread_sleep_for(100);
+ CZustand = 1;
+ }
+ if(CZustand == 1)// Warten
+ {
+ led = 0;
+ //printf("Warten\n");
+ if(pirrechts == 1)
+ {
+ CZustand = 2;
+ printf("erkennt\n");
+ thread_sleep_for(100);
+ //servo_0.SetPosition(2000);
- dist = analogIn.read()*3.3f;
-
- } else {
- // LED on, reset speedController speed, pwm2, position servo
- myled = 1;
-
- speedController_0.setDesiredSpeed(0.0f);
- speedController_1.setDesiredSpeed(0.0f);
- pwm_motor_2.write(0.5f);
-
- servo_desval_0 = 0.0f;
- servo_desval_1 = 0.0f;
- servo_0.SetPosition(servo_desval_0);
- servo_1.SetPosition(servo_desval_1);
-
- dist = analogIn.read()*3.3f;
-
+ }
+ if(pirvorne == 1)
+ {
+ CZustand = 4;
+ }
+ if(pirlinks == 1)
+ {
+ CZustand = 3;
+ }
+ if(pirhinten == 1)
+ {
+ printf("pirhinten\n");
+ CZustand = 5;
+ }
+
+
+
+
+
+ }
+ else{
+
+ }
+
+ if(CZustand == 2)//rechts erkennt
+ {
+ led = 1;
+ printf("rechts\n");
+
+ servo_0.SetPosition(1250); //1250
+ if(ultrarechts == 1)
+ {
+ CZustand = 6;
+ }
+ else{
+ CZustand = 0;
+
}
- // printf("speedLeft: %f, speedRight: %f\r\n",controller.getSpeedLeft(), controller.getSpeedRight());
- // printf("counter1 = %d counter2 = %d counter3 = %d\r\n", encoderCounter_0.read(), encoderCounter_1.read(), encoderCounter_2.read());
- printf("speedController_0 = %d speedController_1 = %d encoderCounter_2 = %d measured value in mV: %d\r\n", static_cast<int>(speedController_0.getSpeed()*1000.0f),
- static_cast<int>(speedController_1.getSpeed()*1000.0f),
- encoderCounter_2.read(),
- (static_cast<int>(dist * 1e3)));
+
+ thread_sleep_for(100);
+ }
+ if(CZustand == 3)//links erkennt
+ {
+ printf("links\n");
+ servo_0.SetPosition(1750);
+ if(ultralinks == 1)
+ {
+ CZustand = 6;
+ }
+ else{
+ CZustand = 0;
+ //thread_sleep_for(100);
+ }
+
+
+ thread_sleep_for(100);
+ }
+ if(CZustand == 4)//vorne erkennt
+ {
+ printf("vorne\n");
+ servo_0.SetPosition(1500);
+ if(ultravorne == 1)
+ {
+ CZustand = 6;
+ }
+ else{
+ CZustand = 0;
+ thread_sleep_for(100);
+ }
+
+
+ thread_sleep_for(100);
+ }
- thread_sleep_for(500);
+ if(CZustand == 5)//hinten erkennt
+ {
+ printf("hinten\n");
+ servo_0.SetPosition(2000);
+ if(ultrahinten == 1)
+ {
+ CZustand = 6;
+ }
+ else{
+ CZustand = 0;
+
+ }
+
+ thread_sleep_for(100);
}
+ if(CZustand == 6)//spritzen
+ {
+ printf("spritzen");
+ summer = 1;
+ pumpe = 1;
+ thread_sleep_for(spritz); // Spritzzeit
+ summer = 0;
+ pumpe = 0;
+ CZustand = 0;
+ servo_0.SetPosition(1500);
+
+
+ }
+
+ /* ------------- stop hacking ------------- -------------*/
+
+ int T_loop_ms = duration_cast<milliseconds>(loop_timer.elapsed_time()).count();
+ int dT_loop_ms = Ts_ms - T_loop_ms;
+ thread_sleep_for(dT_loop_ms);
+
+ }
+
+
+
+
}
+
+