Jack Hansdampf / Mbed OS MiniRoboter4DOF

Dependencies:   LCD_i2c_GSOE

main.cpp@1:f3303de6e057, 2022-01-17 (annotated)

Committer:
jack1930
Date:
Mon Jan 17 16:27:53 2022 +0000
Revision:
1:f3303de6e057
Parent:
0:d7ad0916857f
Child:
2:ddf95aa94826
Kommentare

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 0:d7ad0916857f 25 int main()
jack1930 0:d7ad0916857f 26 {
jack1930 1:f3303de6e057 27 float wg,wg0=140,wg1=100,ag; //Winkel Greifer, Anfangswert, Endwert, Interpolation
jack1930 1:f3303de6e057 28 float wh,wh0=90,wh1=90,ah; //Winkel Heber, Anfangswert, Endwert, Interpolation
jack1930 1:f3303de6e057 29 float ws,ws0=90,ws1=90,as; //Winkel Schwenker, Anfangswert, Endwert, Interpolation
jack1930 1:f3303de6e057 30 float wd,wd0=90,wd1=90,ad; //Winkel Dreher, Anfangswert, Endwert, Interpolation
jack1930 1:f3303de6e057 31 int schritte=50; //Eine Bewegung dauert 50 * 20ms = 1s
jack1930 1:f3303de6e057 32 saft=0b01100110; //Servos mit Strom versorgen.
jack1930 1:f3303de6e057 33 //PWM-Periode einstellen
jack1930 0:d7ad0916857f 34 greifer.period_ms(20);
jack1930 0:d7ad0916857f 35 heber.period_ms(20);
jack1930 0:d7ad0916857f 36 schwenker.period_ms(20);
jack1930 0:d7ad0916857f 37 dreher.period_ms(20);
jack1930 0:d7ad0916857f 38 HAL_Delay(100);
jack1930 1:f3303de6e057 39 //Startposition anfahren Tastgrad PWM 0,025 .. 0,125 für 0 .. 180°
jack1930 0:d7ad0916857f 40 greifer=0.025+(wg0/180.0)*0.1;
jack1930 0:d7ad0916857f 41 heber=0.025+(wh0/180.0)*0.1;
jack1930 0:d7ad0916857f 42 schwenker=0.025+(ws0/180.0)*0.1;
jack1930 0:d7ad0916857f 43 dreher=0.025+(wd0/180.0)*0.1;
jack1930 0:d7ad0916857f 44 mylcd.clear();
jack1930 0:d7ad0916857f 45
jack1930 0:d7ad0916857f 46 while (true) {
jack1930 0:d7ad0916857f 47 mylcd.cursorpos(0);
jack1930 0:d7ad0916857f 48 mylcd.printf("Schritt 0");
jack1930 0:d7ad0916857f 49 schritte=50;
jack1930 1:f3303de6e057 50 //Zielpositionen der Achsen
jack1930 0:d7ad0916857f 51 wg1=180;
jack1930 0:d7ad0916857f 52 wh1=90;
jack1930 0:d7ad0916857f 53 ws1=70;
jack1930 0:d7ad0916857f 54 wd1=150;
jack1930 1:f3303de6e057 55 //Geadeninterpolation
jack1930 0:d7ad0916857f 56 ag=(wg1-wg0)/schritte;
jack1930 0:d7ad0916857f 57 ah=(wh1-wh0)/schritte;
jack1930 0:d7ad0916857f 58 as=(ws1-ws0)/schritte;
jack1930 0:d7ad0916857f 59 ad=(wd1-wd0)/schritte;
jack1930 1:f3303de6e057 60 //Fahren
jack1930 0:d7ad0916857f 61 for (float i=0;i<schritte;i++)
jack1930 0:d7ad0916857f 62 {
jack1930 0:d7ad0916857f 63 wg=ag*i+wg0;
jack1930 0:d7ad0916857f 64 wh=ah*i+wh0;
jack1930 0:d7ad0916857f 65 ws=as*i+ws0;
jack1930 0:d7ad0916857f 66 wd=ad*i+wd0;
jack1930 0:d7ad0916857f 67 greifer=0.025+(wg/180.0)*0.1;
jack1930 0:d7ad0916857f 68 heber=0.025+(wh/180.0)*0.1;
jack1930 0:d7ad0916857f 69 schwenker=0.025+(ws/180.0)*0.1;
jack1930 0:d7ad0916857f 70 dreher=0.025+(wd/180.0)*0.1;
jack1930 0:d7ad0916857f 71 HAL_Delay(20);
jack1930 0:d7ad0916857f 72 }
jack1930 1:f3303de6e057 73 //Startposition für nächsten Schritt = Endposition des vorherigen Schritts
jack1930 0:d7ad0916857f 74 wg0=wg1;
jack1930 0:d7ad0916857f 75 wh0=wh1;
jack1930 0:d7ad0916857f 76 ws0=ws1;
jack1930 0:d7ad0916857f 77 wd0=wd1;
jack1930 0:d7ad0916857f 78
jack1930 0:d7ad0916857f 79 mylcd.cursorpos(0);
jack1930 0:d7ad0916857f 80 mylcd.printf("Schritt 1");
jack1930 0:d7ad0916857f 81
jack1930 0:d7ad0916857f 82 wg1=100;
jack1930 0:d7ad0916857f 83 wh1=110;
jack1930 0:d7ad0916857f 84 ws1=70;
jack1930 0:d7ad0916857f 85 wd1=150;
jack1930 0:d7ad0916857f 86 ag=(wg1-wg0)/schritte;
jack1930 0:d7ad0916857f 87 ah=(wh1-wh0)/schritte;
jack1930 0:d7ad0916857f 88 as=(ws1-ws0)/schritte;
jack1930 0:d7ad0916857f 89 ad=(wd1-wd0)/schritte;
jack1930 0:d7ad0916857f 90 for (float i=0;i<schritte;i++)
jack1930 0:d7ad0916857f 91 {
jack1930 0:d7ad0916857f 92 wg=ag*i+wg0;
jack1930 0:d7ad0916857f 93 wh=ah*i+wh0;
jack1930 0:d7ad0916857f 94 ws=as*i+ws0;
jack1930 0:d7ad0916857f 95 wd=ad*i+wd0;
jack1930 0:d7ad0916857f 96 greifer=0.025+(wg/180.0)*0.1;
jack1930 0:d7ad0916857f 97 heber=0.025+(wh/180.0)*0.1;
jack1930 0:d7ad0916857f 98 schwenker=0.025+(ws/180.0)*0.1;
jack1930 0:d7ad0916857f 99 dreher=0.025+(wd/180.0)*0.1;
jack1930 0:d7ad0916857f 100 HAL_Delay(20);
jack1930 0:d7ad0916857f 101 }
jack1930 0:d7ad0916857f 102
jack1930 0:d7ad0916857f 103 wg0=wg1;
jack1930 0:d7ad0916857f 104 wh0=wh1;
jack1930 0:d7ad0916857f 105 ws0=ws1;
jack1930 0:d7ad0916857f 106 wd0=wd1;
jack1930 0:d7ad0916857f 107
jack1930 0:d7ad0916857f 108 mylcd.cursorpos(0);
jack1930 0:d7ad0916857f 109 mylcd.printf("Schritt 2");
jack1930 0:d7ad0916857f 110
jack1930 0:d7ad0916857f 111 wg1=180;
jack1930 0:d7ad0916857f 112 wh1=110;
jack1930 0:d7ad0916857f 113 ws1=70;
jack1930 0:d7ad0916857f 114 wd1=150;
jack1930 0:d7ad0916857f 115 ag=(wg1-wg0)/schritte;
jack1930 0:d7ad0916857f 116 ah=(wh1-wh0)/schritte;
jack1930 0:d7ad0916857f 117 as=(ws1-ws0)/schritte;
jack1930 0:d7ad0916857f 118 ad=(wd1-wd0)/schritte;
jack1930 0:d7ad0916857f 119 for (float i=0;i<schritte;i++)
jack1930 0:d7ad0916857f 120 {
jack1930 0:d7ad0916857f 121 wg=ag*i+wg0;
jack1930 0:d7ad0916857f 122 wh=ah*i+wh0;
jack1930 0:d7ad0916857f 123 ws=as*i+ws0;
jack1930 0:d7ad0916857f 124 wd=ad*i+wd0;
jack1930 0:d7ad0916857f 125 greifer=0.025+(wg/180.0)*0.1;
jack1930 0:d7ad0916857f 126 heber=0.025+(wh/180.0)*0.1;
jack1930 0:d7ad0916857f 127 schwenker=0.025+(ws/180.0)*0.1;
jack1930 0:d7ad0916857f 128 dreher=0.025+(wd/180.0)*0.1;
jack1930 0:d7ad0916857f 129 HAL_Delay(20);
jack1930 0:d7ad0916857f 130 }
jack1930 0:d7ad0916857f 131
jack1930 0:d7ad0916857f 132 wg0=wg1;
jack1930 0:d7ad0916857f 133 wh0=wh1;
jack1930 0:d7ad0916857f 134 ws0=ws1;
jack1930 0:d7ad0916857f 135 wd0=wd1;
jack1930 0:d7ad0916857f 136
jack1930 0:d7ad0916857f 137 mylcd.cursorpos(0);
jack1930 0:d7ad0916857f 138 mylcd.printf("Schritt 3");
jack1930 0:d7ad0916857f 139
jack1930 0:d7ad0916857f 140 wg1=180;
jack1930 0:d7ad0916857f 141 wh1=60;
jack1930 0:d7ad0916857f 142 ws1=100;
jack1930 0:d7ad0916857f 143 wd1=150;
jack1930 0:d7ad0916857f 144 ag=(wg1-wg0)/schritte;
jack1930 0:d7ad0916857f 145 ah=(wh1-wh0)/schritte;
jack1930 0:d7ad0916857f 146 as=(ws1-ws0)/schritte;
jack1930 0:d7ad0916857f 147 ad=(wd1-wd0)/schritte;
jack1930 0:d7ad0916857f 148 for (float i=0;i<schritte;i++)
jack1930 0:d7ad0916857f 149 {
jack1930 0:d7ad0916857f 150 wg=ag*i+wg0;
jack1930 0:d7ad0916857f 151 wh=ah*i+wh0;
jack1930 0:d7ad0916857f 152 ws=as*i+ws0;
jack1930 0:d7ad0916857f 153 wd=ad*i+wd0;
jack1930 0:d7ad0916857f 154 greifer=0.025+(wg/180.0)*0.1;
jack1930 0:d7ad0916857f 155 heber=0.025+(wh/180.0)*0.1;
jack1930 0:d7ad0916857f 156 schwenker=0.025+(ws/180.0)*0.1;
jack1930 0:d7ad0916857f 157 dreher=0.025+(wd/180.0)*0.1;
jack1930 0:d7ad0916857f 158 HAL_Delay(20);
jack1930 0:d7ad0916857f 159 }
jack1930 0:d7ad0916857f 160
jack1930 0:d7ad0916857f 161 wg0=wg1;
jack1930 0:d7ad0916857f 162 wh0=wh1;
jack1930 0:d7ad0916857f 163 ws0=ws1;
jack1930 0:d7ad0916857f 164 wd0=wd1;
jack1930 0:d7ad0916857f 165
jack1930 0:d7ad0916857f 166 mylcd.cursorpos(0);
jack1930 0:d7ad0916857f 167 mylcd.printf("Schritt 4");
jack1930 0:d7ad0916857f 168
jack1930 0:d7ad0916857f 169 wg1=180;
jack1930 0:d7ad0916857f 170 wh1=30;
jack1930 0:d7ad0916857f 171 ws1=100;
jack1930 0:d7ad0916857f 172 wd1=30;
jack1930 0:d7ad0916857f 173 ag=(wg1-wg0)/schritte;
jack1930 0:d7ad0916857f 174 ah=(wh1-wh0)/schritte;
jack1930 0:d7ad0916857f 175 as=(ws1-ws0)/schritte;
jack1930 0:d7ad0916857f 176 ad=(wd1-wd0)/schritte;
jack1930 0:d7ad0916857f 177 for (float i=0;i<schritte;i++)
jack1930 0:d7ad0916857f 178 {
jack1930 0:d7ad0916857f 179 wg=ag*i+wg0;
jack1930 0:d7ad0916857f 180 wh=ah*i+wh0;
jack1930 0:d7ad0916857f 181 ws=as*i+ws0;
jack1930 0:d7ad0916857f 182 wd=ad*i+wd0;
jack1930 0:d7ad0916857f 183 greifer=0.025+(wg/180.0)*0.1;
jack1930 0:d7ad0916857f 184 heber=0.025+(wh/180.0)*0.1;
jack1930 0:d7ad0916857f 185 schwenker=0.025+(ws/180.0)*0.1;
jack1930 0:d7ad0916857f 186 dreher=0.025+(wd/180.0)*0.1;
jack1930 0:d7ad0916857f 187 HAL_Delay(20);
jack1930 0:d7ad0916857f 188 }
jack1930 0:d7ad0916857f 189
jack1930 0:d7ad0916857f 190 wg0=wg1;
jack1930 0:d7ad0916857f 191 wh0=wh1;
jack1930 0:d7ad0916857f 192 ws0=ws1;
jack1930 0:d7ad0916857f 193 wd0=wd1;
jack1930 0:d7ad0916857f 194
jack1930 0:d7ad0916857f 195 mylcd.cursorpos(0);
jack1930 0:d7ad0916857f 196 mylcd.printf("Schritt 5");
jack1930 0:d7ad0916857f 197
jack1930 0:d7ad0916857f 198 wg1=120;
jack1930 0:d7ad0916857f 199 wh1=30;
jack1930 0:d7ad0916857f 200 ws1=100;
jack1930 0:d7ad0916857f 201 wd1=30;
jack1930 0:d7ad0916857f 202 ag=(wg1-wg0)/schritte;
jack1930 0:d7ad0916857f 203 ah=(wh1-wh0)/schritte;
jack1930 0:d7ad0916857f 204 as=(ws1-ws0)/schritte;
jack1930 0:d7ad0916857f 205 ad=(wd1-wd0)/schritte;
jack1930 0:d7ad0916857f 206 for (float i=0;i<schritte;i++)
jack1930 0:d7ad0916857f 207 {
jack1930 0:d7ad0916857f 208 wg=ag*i+wg0;
jack1930 0:d7ad0916857f 209 wh=ah*i+wh0;
jack1930 0:d7ad0916857f 210 ws=as*i+ws0;
jack1930 0:d7ad0916857f 211 wd=ad*i+wd0;
jack1930 0:d7ad0916857f 212 greifer=0.025+(wg/180.0)*0.1;
jack1930 0:d7ad0916857f 213 heber=0.025+(wh/180.0)*0.1;
jack1930 0:d7ad0916857f 214 schwenker=0.025+(ws/180.0)*0.1;
jack1930 0:d7ad0916857f 215 dreher=0.025+(wd/180.0)*0.1;
jack1930 0:d7ad0916857f 216 HAL_Delay(20);
jack1930 0:d7ad0916857f 217 }
jack1930 0:d7ad0916857f 218
jack1930 0:d7ad0916857f 219 wg0=wg1;
jack1930 0:d7ad0916857f 220 wh0=wh1;
jack1930 0:d7ad0916857f 221 ws0=ws1;
jack1930 0:d7ad0916857f 222 wd0=wd1;
jack1930 0:d7ad0916857f 223
jack1930 0:d7ad0916857f 224 mylcd.cursorpos(0);
jack1930 0:d7ad0916857f 225 mylcd.printf("Schritt 6");
jack1930 0:d7ad0916857f 226
jack1930 0:d7ad0916857f 227 wg1=100;
jack1930 0:d7ad0916857f 228 wh1=60;
jack1930 0:d7ad0916857f 229 ws1=100;
jack1930 0:d7ad0916857f 230 wd1=30;
jack1930 0:d7ad0916857f 231 ag=(wg1-wg0)/schritte;
jack1930 0:d7ad0916857f 232 ah=(wh1-wh0)/schritte;
jack1930 0:d7ad0916857f 233 as=(ws1-ws0)/schritte;
jack1930 0:d7ad0916857f 234 ad=(wd1-wd0)/schritte;
jack1930 0:d7ad0916857f 235 for (float i=0;i<schritte;i++)
jack1930 0:d7ad0916857f 236 {
jack1930 0:d7ad0916857f 237 wg=ag*i+wg0;
jack1930 0:d7ad0916857f 238 wh=ah*i+wh0;
jack1930 0:d7ad0916857f 239 ws=as*i+ws0;
jack1930 0:d7ad0916857f 240 wd=ad*i+wd0;
jack1930 0:d7ad0916857f 241 greifer=0.025+(wg/180.0)*0.1;
jack1930 0:d7ad0916857f 242 heber=0.025+(wh/180.0)*0.1;
jack1930 0:d7ad0916857f 243 schwenker=0.025+(ws/180.0)*0.1;
jack1930 0:d7ad0916857f 244 dreher=0.025+(wd/180.0)*0.1;
jack1930 0:d7ad0916857f 245 HAL_Delay(20);
jack1930 0:d7ad0916857f 246 }
jack1930 0:d7ad0916857f 247
jack1930 0:d7ad0916857f 248 wg0=wg1;
jack1930 0:d7ad0916857f 249 wh0=wh1;
jack1930 0:d7ad0916857f 250 ws0=ws1;
jack1930 0:d7ad0916857f 251 wd0=wd1;
jack1930 0:d7ad0916857f 252 }
jack1930 0:d7ad0916857f 253 }