File content as of revision 2:ddf95aa94826:

/* 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


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()
{
    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;
        go();
        
        mylcd.cursorpos(0);
        mylcd.printf("Schritt 1");

        wg1=100;
        wh1=110;
        ws1=70;
        wd1=150;
        go();
        

        mylcd.cursorpos(0);
        mylcd.printf("Schritt 2");     

        wg1=180;
        wh1=110;
        ws1=70;
        wd1=150;
        go();
        

        mylcd.cursorpos(0);
        mylcd.printf("Schritt 3");

        wg1=180;
        wh1=60;
        ws1=100;
        wd1=150;
        go();
 
        mylcd.cursorpos(0);
        mylcd.printf("Schritt 4");

        wg1=180;
        wh1=30;
        ws1=100;
        wd1=30;
        go();
        
        mylcd.cursorpos(0);
        mylcd.printf("Schritt 5");
 
        wg1=120;
        wh1=30;
        ws1=100;
        wd1=30;
        go();
        
        mylcd.cursorpos(0);
        mylcd.printf("Schritt 6");
 
        wg1=100;
        wh1=60;
        ws1=100;
        wd1=30;
        go();
    }
}