Jack Hansdampf / Mbed OS MiniRoboter4DOF

Miniroboter 4DOF mit SG90 Servos an NucleoL152

Dependencies:   LCD_i2c_GSOE

main.cpp@2:ddf95aa94826, 2022-01-18 (annotated)

Committer:
jack1930
Date:
Tue Jan 18 12:05:29 2022 +0000
Revision:
2:ddf95aa94826
Parent:
1:f3303de6e057 
Operation go() eingebaut

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jack1930 0:d7ad0916857f 1 /* mbed Microcontroller Library
jack1930 0:d7ad0916857f 2 * Copyright (c) 2019 ARM Limited
jack1930 0:d7ad0916857f 3 * SPDX-License-Identifier: Apache-2.0
jack1930 0:d7ad0916857f 4 */
jack1930 1:f3303de6e057 5 /* Auf dem Baseshield sind 3 Jumperbrücken erforderlich:
jack1930 1:f3303de6e057 6 * PB9 - PC0
jack1930 1:f3303de6e057 7 * PB3 - PC3
jack1930 1:f3303de6e057 8 * PB4 - PC4
jack1930 1:f3303de6e057 9 * Für die Stromversorgung der L293 wird unter Umständen eine 6V Batteriebox benötigt
jack1930 1:f3303de6e057 10 */
jack1930 1:f3303de6e057 11
jack1930 0:d7ad0916857f 12 #include "mbed.h"
jack1930 0:d7ad0916857f 13 #include "LCD.h"
jack1930 0:d7ad0916857f 14
jack1930 0:d7ad0916857f 15 lcd mylcd;
jack1930 0:d7ad0916857f 16
jack1930 1:f3303de6e057 17 PwmOut greifer(PC_7);
jack1930 1:f3303de6e057 18 PwmOut heber(PB_3);
jack1930 0:d7ad0916857f 19 PwmOut schwenker(PB_4);
jack1930 0:d7ad0916857f 20 PwmOut dreher(PB_9);
jack1930 0:d7ad0916857f 21
jack1930 1:f3303de6e057 22 PortOut saft(PortC,0b01100110); //Stromversorgung der Servos über den L293D
jack1930 1:f3303de6e057 23 AnalogIn poti(PA_0); //wird nicht gebraucht
jack1930 0:d7ad0916857f 24
jack1930 2:ddf95aa94826 25
jack1930 2:ddf95aa94826 26 float wg,wg0=140,wg1=100,ag; //Winkel Greifer, Anfangswert, Endwert, Interpolation
jack1930 2:ddf95aa94826 27 float wh,wh0=90,wh1=90,ah; //Winkel Heber, Anfangswert, Endwert, Interpolation
jack1930 2:ddf95aa94826 28 float ws,ws0=90,ws1=90,as; //Winkel Schwenker, Anfangswert, Endwert, Interpolation
jack1930 2:ddf95aa94826 29 float wd,wd0=90,wd1=90,ad; //Winkel Dreher, Anfangswert, Endwert, Interpolation
jack1930 2:ddf95aa94826 30 int schritte=50; //Eine Bewegung dauert 50 * 20ms = 1s
jack1930 2:ddf95aa94826 31
jack1930 2:ddf95aa94826 32 void go()
jack1930 2:ddf95aa94826 33 {
jack1930 2:ddf95aa94826 34 //Geadeninterpolation
jack1930 2:ddf95aa94826 35 ag=(wg1-wg0)/schritte;
jack1930 2:ddf95aa94826 36 ah=(wh1-wh0)/schritte;
jack1930 2:ddf95aa94826 37 as=(ws1-ws0)/schritte;
jack1930 2:ddf95aa94826 38 ad=(wd1-wd0)/schritte;
jack1930 2:ddf95aa94826 39 //Fahren
jack1930 2:ddf95aa94826 40 for (float i=0;i<schritte;i++)
jack1930 2:ddf95aa94826 41 {
jack1930 2:ddf95aa94826 42 wg=ag*i+wg0;
jack1930 2:ddf95aa94826 43 wh=ah*i+wh0;
jack1930 2:ddf95aa94826 44 ws=as*i+ws0;
jack1930 2:ddf95aa94826 45 wd=ad*i+wd0;
jack1930 2:ddf95aa94826 46 greifer=0.025+(wg/180.0)*0.1;
jack1930 2:ddf95aa94826 47 heber=0.025+(wh/180.0)*0.1;
jack1930 2:ddf95aa94826 48 schwenker=0.025+(ws/180.0)*0.1;
jack1930 2:ddf95aa94826 49 dreher=0.025+(wd/180.0)*0.1;
jack1930 2:ddf95aa94826 50 HAL_Delay(20);
jack1930 2:ddf95aa94826 51 }
jack1930 2:ddf95aa94826 52 //Startposition für nächsten Schritt = Endposition des vorherigen Schritts
jack1930 2:ddf95aa94826 53 wg0=wg1;
jack1930 2:ddf95aa94826 54 wh0=wh1;
jack1930 2:ddf95aa94826 55 ws0=ws1;
jack1930 2:ddf95aa94826 56 wd0=wd1;
jack1930 2:ddf95aa94826 57 }
jack1930 2:ddf95aa94826 58
jack1930 0:d7ad0916857f 59 int main()
jack1930 0:d7ad0916857f 60 {
jack1930 1:f3303de6e057 61 int schritte=50; //Eine Bewegung dauert 50 * 20ms = 1s
jack1930 1:f3303de6e057 62 saft=0b01100110; //Servos mit Strom versorgen.
jack1930 1:f3303de6e057 63 //PWM-Periode einstellen
jack1930 0:d7ad0916857f 64 greifer.period_ms(20);
jack1930 0:d7ad0916857f 65 heber.period_ms(20);
jack1930 0:d7ad0916857f 66 schwenker.period_ms(20);
jack1930 0:d7ad0916857f 67 dreher.period_ms(20);
jack1930 0:d7ad0916857f 68 HAL_Delay(100);
jack1930 1:f3303de6e057 69 //Startposition anfahren Tastgrad PWM 0,025 .. 0,125 für 0 .. 180°
jack1930 0:d7ad0916857f 70 greifer=0.025+(wg0/180.0)*0.1;
jack1930 0:d7ad0916857f 71 heber=0.025+(wh0/180.0)*0.1;
jack1930 0:d7ad0916857f 72 schwenker=0.025+(ws0/180.0)*0.1;
jack1930 0:d7ad0916857f 73 dreher=0.025+(wd0/180.0)*0.1;
jack1930 0:d7ad0916857f 74 mylcd.clear();
jack1930 0:d7ad0916857f 75
jack1930 0:d7ad0916857f 76 while (true) {
jack1930 0:d7ad0916857f 77 mylcd.cursorpos(0);
jack1930 0:d7ad0916857f 78 mylcd.printf("Schritt 0");
jack1930 0:d7ad0916857f 79 schritte=50;
jack1930 1:f3303de6e057 80 //Zielpositionen der Achsen
jack1930 0:d7ad0916857f 81 wg1=180;
jack1930 0:d7ad0916857f 82 wh1=90;
jack1930 0:d7ad0916857f 83 ws1=70;
jack1930 0:d7ad0916857f 84 wd1=150;
jack1930 2:ddf95aa94826 85 go();
jack1930 2:ddf95aa94826 86
jack1930 0:d7ad0916857f 87 mylcd.cursorpos(0);
jack1930 0:d7ad0916857f 88 mylcd.printf("Schritt 1");
jack1930 0:d7ad0916857f 89
jack1930 0:d7ad0916857f 90 wg1=100;
jack1930 0:d7ad0916857f 91 wh1=110;
jack1930 0:d7ad0916857f 92 ws1=70;
jack1930 0:d7ad0916857f 93 wd1=150;
jack1930 2:ddf95aa94826 94 go();
jack1930 2:ddf95aa94826 95
jack1930 0:d7ad0916857f 96
jack1930 0:d7ad0916857f 97 mylcd.cursorpos(0);
jack1930 0:d7ad0916857f 98 mylcd.printf("Schritt 2");
jack1930 0:d7ad0916857f 99
jack1930 0:d7ad0916857f 100 wg1=180;
jack1930 0:d7ad0916857f 101 wh1=110;
jack1930 0:d7ad0916857f 102 ws1=70;
jack1930 0:d7ad0916857f 103 wd1=150;
jack1930 2:ddf95aa94826 104 go();
jack1930 2:ddf95aa94826 105
jack1930 0:d7ad0916857f 106
jack1930 0:d7ad0916857f 107 mylcd.cursorpos(0);
jack1930 0:d7ad0916857f 108 mylcd.printf("Schritt 3");
jack1930 0:d7ad0916857f 109
jack1930 0:d7ad0916857f 110 wg1=180;
jack1930 0:d7ad0916857f 111 wh1=60;
jack1930 0:d7ad0916857f 112 ws1=100;
jack1930 0:d7ad0916857f 113 wd1=150;
jack1930 2:ddf95aa94826 114 go();
jack1930 0:d7ad0916857f 115
jack1930 0:d7ad0916857f 116 mylcd.cursorpos(0);
jack1930 0:d7ad0916857f 117 mylcd.printf("Schritt 4");
jack1930 0:d7ad0916857f 118
jack1930 0:d7ad0916857f 119 wg1=180;
jack1930 0:d7ad0916857f 120 wh1=30;
jack1930 0:d7ad0916857f 121 ws1=100;
jack1930 0:d7ad0916857f 122 wd1=30;
jack1930 2:ddf95aa94826 123 go();
jack1930 0:d7ad0916857f 124
jack1930 0:d7ad0916857f 125 mylcd.cursorpos(0);
jack1930 0:d7ad0916857f 126 mylcd.printf("Schritt 5");
jack1930 0:d7ad0916857f 127
jack1930 0:d7ad0916857f 128 wg1=120;
jack1930 0:d7ad0916857f 129 wh1=30;
jack1930 0:d7ad0916857f 130 ws1=100;
jack1930 0:d7ad0916857f 131 wd1=30;
jack1930 2:ddf95aa94826 132 go();
jack1930 0:d7ad0916857f 133
jack1930 0:d7ad0916857f 134 mylcd.cursorpos(0);
jack1930 0:d7ad0916857f 135 mylcd.printf("Schritt 6");
jack1930 0:d7ad0916857f 136
jack1930 0:d7ad0916857f 137 wg1=100;
jack1930 0:d7ad0916857f 138 wh1=60;
jack1930 0:d7ad0916857f 139 ws1=100;
jack1930 0:d7ad0916857f 140 wd1=30;
jack1930 2:ddf95aa94826 141 go();
jack1930 0:d7ad0916857f 142 }
jack1930 0:d7ad0916857f 143 }