Lehrer Busch
/
xxx_MiniRoboter4DOF
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
main.cpp
- Committer:
- jack1930
- Date:
- 2022-01-17
- Revision:
- 1:f3303de6e057
- Parent:
- 0:d7ad0916857f
- Child:
- 2:ddf95aa94826
File content as of revision 1:f3303de6e057:
/* mbed Microcontroller Library
* Copyright (c) 2019 ARM Limited
* SPDX-License-Identifier: Apache-2.0
*/
/* Auf dem Baseshield sind 3 Jumperbrücken erforderlich:
* PB9 - PC0
* PB3 - PC3
* PB4 - PC4
* Für die Stromversorgung der L293 wird unter Umständen eine 6V Batteriebox benötigt
*/
#include "mbed.h"
#include "LCD.h"
lcd mylcd;
PwmOut greifer(PC_7);
PwmOut heber(PB_3);
PwmOut schwenker(PB_4);
PwmOut dreher(PB_9);
PortOut saft(PortC,0b01100110); //Stromversorgung der Servos über den L293D
AnalogIn poti(PA_0); //wird nicht gebraucht
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
greifer.period_ms(20);
heber.period_ms(20);
schwenker.period_ms(20);
dreher.period_ms(20);
HAL_Delay(100);
//Startposition anfahren Tastgrad PWM 0,025 .. 0,125 für 0 .. 180°
greifer=0.025+(wg0/180.0)*0.1;
heber=0.025+(wh0/180.0)*0.1;
schwenker=0.025+(ws0/180.0)*0.1;
dreher=0.025+(wd0/180.0)*0.1;
mylcd.clear();
while (true) {
mylcd.cursorpos(0);
mylcd.printf("Schritt 0");
schritte=50;
//Zielpositionen der Achsen
wg1=180;
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;
mylcd.cursorpos(0);
mylcd.printf("Schritt 1");
wg1=100;
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;
mylcd.cursorpos(0);
mylcd.printf("Schritt 2");
wg1=180;
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;
mylcd.cursorpos(0);
mylcd.printf("Schritt 3");
wg1=180;
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;
mylcd.cursorpos(0);
mylcd.printf("Schritt 4");
wg1=180;
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;
mylcd.cursorpos(0);
mylcd.printf("Schritt 5");
wg1=120;
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;
mylcd.cursorpos(0);
mylcd.printf("Schritt 6");
wg1=100;
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;
}
}