main.cpp

00001 /* mbed Microcontroller Library
00002  * Copyright (c) 2019 ARM Limited
00003  * SPDX-License-Identifier: Apache-2.0
00004  */
00005 /* Auf dem Baseshield sind 3 Jumperbrücken erforderlich:
00006  * PB9 - PC0
00007  * PB3 - PC3
00008  * PB4 - PC4
00009  * Für die Stromversorgung der L293 wird unter Umständen eine 6V Batteriebox benötigt
00010  */
00011  
00012 #include "mbed.h"
00013 #include "LCD.h"
00014 
00015 lcd mylcd;
00016 
00017 PwmOut greifer(PC_7);   
00018 PwmOut heber(PB_3);     
00019 PwmOut schwenker(PB_4);
00020 PwmOut dreher(PB_9);
00021 
00022 PortOut saft(PortC,0b01100110); //Stromversorgung der Servos über den L293D
00023 AnalogIn poti(PA_0); //wird nicht gebraucht
00024 
00025 
00026 float wg,wg0=140,wg1=100,ag; //Winkel Greifer, Anfangswert, Endwert, Interpolation
00027 float wh,wh0=90,wh1=90,ah;  //Winkel Heber, Anfangswert, Endwert, Interpolation
00028 float ws,ws0=90,ws1=90,as;  //Winkel Schwenker, Anfangswert, Endwert, Interpolation
00029 float wd,wd0=90,wd1=90,ad;  //Winkel Dreher, Anfangswert, Endwert, Interpolation
00030 int schritte=50;    //Eine Bewegung dauert 50 * 20ms = 1s
00031 
00032 void go()
00033 {
00034          //Geadeninterpolation
00035         ag=(wg1-wg0)/schritte; 
00036         ah=(wh1-wh0)/schritte;   
00037         as=(ws1-ws0)/schritte;  
00038         ad=(wd1-wd0)/schritte;   
00039         //Fahren
00040         for (float i=0;i<schritte;i++)
00041         {   
00042             wg=ag*i+wg0;
00043             wh=ah*i+wh0;
00044             ws=as*i+ws0;
00045             wd=ad*i+wd0;
00046             greifer=0.025+(wg/180.0)*0.1;
00047             heber=0.025+(wh/180.0)*0.1;  
00048             schwenker=0.025+(ws/180.0)*0.1;   
00049             dreher=0.025+(wd/180.0)*0.1;        
00050             HAL_Delay(20);
00051         }
00052         //Startposition für nächsten Schritt = Endposition des vorherigen Schritts
00053         wg0=wg1;
00054         wh0=wh1;
00055         ws0=ws1;
00056         wd0=wd1;   
00057 }
00058 
00059 int main()
00060 {
00061     int schritte=50;    //Eine Bewegung dauert 50 * 20ms = 1s
00062     saft=0b01100110;    //Servos mit Strom versorgen. 
00063     //PWM-Periode einstellen
00064     greifer.period_ms(20);
00065     heber.period_ms(20);
00066     schwenker.period_ms(20);
00067     dreher.period_ms(20);
00068     HAL_Delay(100);
00069     //Startposition anfahren Tastgrad PWM 0,025 .. 0,125 für 0 .. 180°
00070     greifer=0.025+(wg0/180.0)*0.1;
00071     heber=0.025+(wh0/180.0)*0.1;  
00072     schwenker=0.025+(ws0/180.0)*0.1;   
00073     dreher=0.025+(wd0/180.0)*0.1;   
00074     mylcd.clear();
00075     
00076     while (true) {
00077         mylcd.cursorpos(0);
00078         mylcd.printf("Schritt 0");
00079         schritte=50;
00080         //Zielpositionen der Achsen
00081         wg1=180;
00082         wh1=90;
00083         ws1=70;
00084         wd1=150;
00085         go();
00086         
00087         mylcd.cursorpos(0);
00088         mylcd.printf("Schritt 1");
00089 
00090         wg1=100;
00091         wh1=110;
00092         ws1=70;
00093         wd1=150;
00094         go();
00095         
00096 
00097         mylcd.cursorpos(0);
00098         mylcd.printf("Schritt 2");     
00099 
00100         wg1=180;
00101         wh1=110;
00102         ws1=70;
00103         wd1=150;
00104         go();
00105         
00106 
00107         mylcd.cursorpos(0);
00108         mylcd.printf("Schritt 3");
00109 
00110         wg1=180;
00111         wh1=60;
00112         ws1=100;
00113         wd1=150;
00114         go();
00115  
00116         mylcd.cursorpos(0);
00117         mylcd.printf("Schritt 4");
00118 
00119         wg1=180;
00120         wh1=30;
00121         ws1=100;
00122         wd1=30;
00123         go();
00124         
00125         mylcd.cursorpos(0);
00126         mylcd.printf("Schritt 5");
00127  
00128         wg1=120;
00129         wh1=30;
00130         ws1=100;
00131         wd1=30;
00132         go();
00133         
00134         mylcd.cursorpos(0);
00135         mylcd.printf("Schritt 6");
00136  
00137         wg1=100;
00138         wh1=60;
00139         ws1=100;
00140         wd1=30;
00141         go();
00142     }
00143 }