Diff: main.cpp
- Revision:
- 2:ddf95aa94826
- Parent:
- 1:f3303de6e057
--- a/main.cpp Mon Jan 17 16:27:53 2022 +0000
+++ b/main.cpp Tue Jan 18 12:05:29 2022 +0000
@@ -22,12 +22,42 @@
PortOut saft(PortC,0b01100110); //Stromversorgung der Servos über den L293D
AnalogIn poti(PA_0); //wird nicht gebraucht
+
+float wg,wg0=140,wg1=100,ag; //Winkel Greifer, Anfangswert, Endwert, Interpolation
+float wh,wh0=90,wh1=90,ah; //Winkel Heber, Anfangswert, Endwert, Interpolation
+float ws,ws0=90,ws1=90,as; //Winkel Schwenker, Anfangswert, Endwert, Interpolation
+float wd,wd0=90,wd1=90,ad; //Winkel Dreher, Anfangswert, Endwert, Interpolation
+int schritte=50; //Eine Bewegung dauert 50 * 20ms = 1s
+
+void go()
+{
+ //Geadeninterpolation
+ ag=(wg1-wg0)/schritte;
+ ah=(wh1-wh0)/schritte;
+ as=(ws1-ws0)/schritte;
+ ad=(wd1-wd0)/schritte;
+ //Fahren
+ for (float i=0;i<schritte;i++)
+ {
+ wg=ag*i+wg0;
+ wh=ah*i+wh0;
+ ws=as*i+ws0;
+ wd=ad*i+wd0;
+ greifer=0.025+(wg/180.0)*0.1;
+ heber=0.025+(wh/180.0)*0.1;
+ schwenker=0.025+(ws/180.0)*0.1;
+ dreher=0.025+(wd/180.0)*0.1;
+ HAL_Delay(20);
+ }
+ //Startposition für nächsten Schritt = Endposition des vorherigen Schritts
+ wg0=wg1;
+ wh0=wh1;
+ ws0=ws1;
+ wd0=wd1;
+}
+
int main()
{
- float wg,wg0=140,wg1=100,ag; //Winkel Greifer, Anfangswert, Endwert, Interpolation
- float wh,wh0=90,wh1=90,ah; //Winkel Heber, Anfangswert, Endwert, Interpolation
- float ws,ws0=90,ws1=90,as; //Winkel Schwenker, Anfangswert, Endwert, Interpolation
- float wd,wd0=90,wd1=90,ad; //Winkel Dreher, Anfangswert, Endwert, Interpolation
int schritte=50; //Eine Bewegung dauert 50 * 20ms = 1s
saft=0b01100110; //Servos mit Strom versorgen.
//PWM-Periode einstellen
@@ -52,30 +82,8 @@
wh1=90;
ws1=70;
wd1=150;
- //Geadeninterpolation
- ag=(wg1-wg0)/schritte;
- ah=(wh1-wh0)/schritte;
- as=(ws1-ws0)/schritte;
- ad=(wd1-wd0)/schritte;
- //Fahren
- for (float i=0;i<schritte;i++)
- {
- wg=ag*i+wg0;
- wh=ah*i+wh0;
- ws=as*i+ws0;
- wd=ad*i+wd0;
- greifer=0.025+(wg/180.0)*0.1;
- heber=0.025+(wh/180.0)*0.1;
- schwenker=0.025+(ws/180.0)*0.1;
- dreher=0.025+(wd/180.0)*0.1;
- HAL_Delay(20);
- }
- //Startposition für nächsten Schritt = Endposition des vorherigen Schritts
- wg0=wg1;
- wh0=wh1;
- ws0=ws1;
- wd0=wd1;
-
+ go();
+
mylcd.cursorpos(0);
mylcd.printf("Schritt 1");
@@ -83,27 +91,8 @@
wh1=110;
ws1=70;
wd1=150;
- ag=(wg1-wg0)/schritte;
- ah=(wh1-wh0)/schritte;
- as=(ws1-ws0)/schritte;
- ad=(wd1-wd0)/schritte;
- for (float i=0;i<schritte;i++)
- {
- wg=ag*i+wg0;
- wh=ah*i+wh0;
- ws=as*i+ws0;
- wd=ad*i+wd0;
- greifer=0.025+(wg/180.0)*0.1;
- heber=0.025+(wh/180.0)*0.1;
- schwenker=0.025+(ws/180.0)*0.1;
- dreher=0.025+(wd/180.0)*0.1;
- HAL_Delay(20);
- }
-
- wg0=wg1;
- wh0=wh1;
- ws0=ws1;
- wd0=wd1;
+ go();
+
mylcd.cursorpos(0);
mylcd.printf("Schritt 2");
@@ -112,27 +101,8 @@
wh1=110;
ws1=70;
wd1=150;
- ag=(wg1-wg0)/schritte;
- ah=(wh1-wh0)/schritte;
- as=(ws1-ws0)/schritte;
- ad=(wd1-wd0)/schritte;
- for (float i=0;i<schritte;i++)
- {
- wg=ag*i+wg0;
- wh=ah*i+wh0;
- ws=as*i+ws0;
- wd=ad*i+wd0;
- greifer=0.025+(wg/180.0)*0.1;
- heber=0.025+(wh/180.0)*0.1;
- schwenker=0.025+(ws/180.0)*0.1;
- dreher=0.025+(wd/180.0)*0.1;
- HAL_Delay(20);
- }
-
- wg0=wg1;
- wh0=wh1;
- ws0=ws1;
- wd0=wd1;
+ go();
+
mylcd.cursorpos(0);
mylcd.printf("Schritt 3");
@@ -141,27 +111,7 @@
wh1=60;
ws1=100;
wd1=150;
- ag=(wg1-wg0)/schritte;
- ah=(wh1-wh0)/schritte;
- as=(ws1-ws0)/schritte;
- ad=(wd1-wd0)/schritte;
- for (float i=0;i<schritte;i++)
- {
- wg=ag*i+wg0;
- wh=ah*i+wh0;
- ws=as*i+ws0;
- wd=ad*i+wd0;
- greifer=0.025+(wg/180.0)*0.1;
- heber=0.025+(wh/180.0)*0.1;
- schwenker=0.025+(ws/180.0)*0.1;
- dreher=0.025+(wd/180.0)*0.1;
- HAL_Delay(20);
- }
-
- wg0=wg1;
- wh0=wh1;
- ws0=ws1;
- wd0=wd1;
+ go();
mylcd.cursorpos(0);
mylcd.printf("Schritt 4");
@@ -170,27 +120,7 @@
wh1=30;
ws1=100;
wd1=30;
- ag=(wg1-wg0)/schritte;
- ah=(wh1-wh0)/schritte;
- as=(ws1-ws0)/schritte;
- ad=(wd1-wd0)/schritte;
- for (float i=0;i<schritte;i++)
- {
- wg=ag*i+wg0;
- wh=ah*i+wh0;
- ws=as*i+ws0;
- wd=ad*i+wd0;
- greifer=0.025+(wg/180.0)*0.1;
- heber=0.025+(wh/180.0)*0.1;
- schwenker=0.025+(ws/180.0)*0.1;
- dreher=0.025+(wd/180.0)*0.1;
- HAL_Delay(20);
- }
-
- wg0=wg1;
- wh0=wh1;
- ws0=ws1;
- wd0=wd1;
+ go();
mylcd.cursorpos(0);
mylcd.printf("Schritt 5");
@@ -199,27 +129,7 @@
wh1=30;
ws1=100;
wd1=30;
- ag=(wg1-wg0)/schritte;
- ah=(wh1-wh0)/schritte;
- as=(ws1-ws0)/schritte;
- ad=(wd1-wd0)/schritte;
- for (float i=0;i<schritte;i++)
- {
- wg=ag*i+wg0;
- wh=ah*i+wh0;
- ws=as*i+ws0;
- wd=ad*i+wd0;
- greifer=0.025+(wg/180.0)*0.1;
- heber=0.025+(wh/180.0)*0.1;
- schwenker=0.025+(ws/180.0)*0.1;
- dreher=0.025+(wd/180.0)*0.1;
- HAL_Delay(20);
- }
-
- wg0=wg1;
- wh0=wh1;
- ws0=ws1;
- wd0=wd1;
+ go();
mylcd.cursorpos(0);
mylcd.printf("Schritt 6");
@@ -228,26 +138,6 @@
wh1=60;
ws1=100;
wd1=30;
- ag=(wg1-wg0)/schritte;
- ah=(wh1-wh0)/schritte;
- as=(ws1-ws0)/schritte;
- ad=(wd1-wd0)/schritte;
- for (float i=0;i<schritte;i++)
- {
- wg=ag*i+wg0;
- wh=ah*i+wh0;
- ws=as*i+ws0;
- wd=ad*i+wd0;
- greifer=0.025+(wg/180.0)*0.1;
- heber=0.025+(wh/180.0)*0.1;
- schwenker=0.025+(ws/180.0)*0.1;
- dreher=0.025+(wd/180.0)*0.1;
- HAL_Delay(20);
- }
-
- wg0=wg1;
- wh0=wh1;
- ws0=ws1;
- wd0=wd1;
+ go();
}
}