Jack Hansdampf / Mbed OS MiniRoboter4DOF

Miniroboter 4DOF mit SG90 Servos an NucleoL152

Dependencies:   LCD_i2c_GSOE

main.cpp@0:d7ad0916857f, 2022-01-17 (annotated)

Committer:
jack1930
Date:
Mon Jan 17 15:55:17 2022 +0000
Revision:
0:d7ad0916857f
Child:
1:f3303de6e057
Miniroboter 4DOF mit SG90 Servos

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 0:d7ad0916857f 5
jack1930 0:d7ad0916857f 6 #include "mbed.h"
jack1930 0:d7ad0916857f 7 #include "LCD.h"
jack1930 0:d7ad0916857f 8
jack1930 0:d7ad0916857f 9 lcd mylcd;
jack1930 0:d7ad0916857f 10
jack1930 0:d7ad0916857f 11 PwmOut greifer(PC_7);
jack1930 0:d7ad0916857f 12 PwmOut heber(PB_3);
jack1930 0:d7ad0916857f 13 PwmOut schwenker(PB_4);
jack1930 0:d7ad0916857f 14 PwmOut dreher(PB_9);
jack1930 0:d7ad0916857f 15
jack1930 0:d7ad0916857f 16 PortOut saft(PortC,0b01100110);
jack1930 0:d7ad0916857f 17 AnalogIn poti(PA_0);
jack1930 0:d7ad0916857f 18
jack1930 0:d7ad0916857f 19 int main()
jack1930 0:d7ad0916857f 20 {
jack1930 0:d7ad0916857f 21 float wg,wg0=140,wg1=100,ag;
jack1930 0:d7ad0916857f 22 float wh,wh0=90,wh1=90,ah;
jack1930 0:d7ad0916857f 23 float ws,ws0=90,ws1=90,as;
jack1930 0:d7ad0916857f 24 float wd,wd0=90,wd1=90,ad;
jack1930 0:d7ad0916857f 25 int schritte=50;
jack1930 0:d7ad0916857f 26 saft=0b01100110;
jack1930 0:d7ad0916857f 27 greifer.period_ms(20);
jack1930 0:d7ad0916857f 28 heber.period_ms(20);
jack1930 0:d7ad0916857f 29 schwenker.period_ms(20);
jack1930 0:d7ad0916857f 30 dreher.period_ms(20);
jack1930 0:d7ad0916857f 31 HAL_Delay(100);
jack1930 0:d7ad0916857f 32 greifer=0.025+(wg0/180.0)*0.1;
jack1930 0:d7ad0916857f 33 heber=0.025+(wh0/180.0)*0.1;
jack1930 0:d7ad0916857f 34 schwenker=0.025+(ws0/180.0)*0.1;
jack1930 0:d7ad0916857f 35 dreher=0.025+(wd0/180.0)*0.1;
jack1930 0:d7ad0916857f 36 mylcd.clear();
jack1930 0:d7ad0916857f 37
jack1930 0:d7ad0916857f 38 while (true) {
jack1930 0:d7ad0916857f 39 mylcd.cursorpos(0);
jack1930 0:d7ad0916857f 40 mylcd.printf("Schritt 0");
jack1930 0:d7ad0916857f 41 schritte=50;
jack1930 0:d7ad0916857f 42 wg1=180;
jack1930 0:d7ad0916857f 43 wh1=90;
jack1930 0:d7ad0916857f 44 ws1=70;
jack1930 0:d7ad0916857f 45 wd1=150;
jack1930 0:d7ad0916857f 46 ag=(wg1-wg0)/schritte;
jack1930 0:d7ad0916857f 47 ah=(wh1-wh0)/schritte;
jack1930 0:d7ad0916857f 48 as=(ws1-ws0)/schritte;
jack1930 0:d7ad0916857f 49 ad=(wd1-wd0)/schritte;
jack1930 0:d7ad0916857f 50 for (float i=0;i<schritte;i++)
jack1930 0:d7ad0916857f 51 {
jack1930 0:d7ad0916857f 52 wg=ag*i+wg0;
jack1930 0:d7ad0916857f 53 wh=ah*i+wh0;
jack1930 0:d7ad0916857f 54 ws=as*i+ws0;
jack1930 0:d7ad0916857f 55 wd=ad*i+wd0;
jack1930 0:d7ad0916857f 56 greifer=0.025+(wg/180.0)*0.1;
jack1930 0:d7ad0916857f 57 heber=0.025+(wh/180.0)*0.1;
jack1930 0:d7ad0916857f 58 schwenker=0.025+(ws/180.0)*0.1;
jack1930 0:d7ad0916857f 59 dreher=0.025+(wd/180.0)*0.1;
jack1930 0:d7ad0916857f 60 HAL_Delay(20);
jack1930 0:d7ad0916857f 61 }
jack1930 0:d7ad0916857f 62
jack1930 0:d7ad0916857f 63 wg0=wg1;
jack1930 0:d7ad0916857f 64 wh0=wh1;
jack1930 0:d7ad0916857f 65 ws0=ws1;
jack1930 0:d7ad0916857f 66 wd0=wd1;
jack1930 0:d7ad0916857f 67
jack1930 0:d7ad0916857f 68 mylcd.cursorpos(0);
jack1930 0:d7ad0916857f 69 mylcd.printf("Schritt 1");
jack1930 0:d7ad0916857f 70
jack1930 0:d7ad0916857f 71 wg1=100;
jack1930 0:d7ad0916857f 72 wh1=110;
jack1930 0:d7ad0916857f 73 ws1=70;
jack1930 0:d7ad0916857f 74 wd1=150;
jack1930 0:d7ad0916857f 75 ag=(wg1-wg0)/schritte;
jack1930 0:d7ad0916857f 76 ah=(wh1-wh0)/schritte;
jack1930 0:d7ad0916857f 77 as=(ws1-ws0)/schritte;
jack1930 0:d7ad0916857f 78 ad=(wd1-wd0)/schritte;
jack1930 0:d7ad0916857f 79 for (float i=0;i<schritte;i++)
jack1930 0:d7ad0916857f 80 {
jack1930 0:d7ad0916857f 81 wg=ag*i+wg0;
jack1930 0:d7ad0916857f 82 wh=ah*i+wh0;
jack1930 0:d7ad0916857f 83 ws=as*i+ws0;
jack1930 0:d7ad0916857f 84 wd=ad*i+wd0;
jack1930 0:d7ad0916857f 85 greifer=0.025+(wg/180.0)*0.1;
jack1930 0:d7ad0916857f 86 heber=0.025+(wh/180.0)*0.1;
jack1930 0:d7ad0916857f 87 schwenker=0.025+(ws/180.0)*0.1;
jack1930 0:d7ad0916857f 88 dreher=0.025+(wd/180.0)*0.1;
jack1930 0:d7ad0916857f 89 HAL_Delay(20);
jack1930 0:d7ad0916857f 90 }
jack1930 0:d7ad0916857f 91
jack1930 0:d7ad0916857f 92 wg0=wg1;
jack1930 0:d7ad0916857f 93 wh0=wh1;
jack1930 0:d7ad0916857f 94 ws0=ws1;
jack1930 0:d7ad0916857f 95 wd0=wd1;
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 0:d7ad0916857f 104 ag=(wg1-wg0)/schritte;
jack1930 0:d7ad0916857f 105 ah=(wh1-wh0)/schritte;
jack1930 0:d7ad0916857f 106 as=(ws1-ws0)/schritte;
jack1930 0:d7ad0916857f 107 ad=(wd1-wd0)/schritte;
jack1930 0:d7ad0916857f 108 for (float i=0;i<schritte;i++)
jack1930 0:d7ad0916857f 109 {
jack1930 0:d7ad0916857f 110 wg=ag*i+wg0;
jack1930 0:d7ad0916857f 111 wh=ah*i+wh0;
jack1930 0:d7ad0916857f 112 ws=as*i+ws0;
jack1930 0:d7ad0916857f 113 wd=ad*i+wd0;
jack1930 0:d7ad0916857f 114 greifer=0.025+(wg/180.0)*0.1;
jack1930 0:d7ad0916857f 115 heber=0.025+(wh/180.0)*0.1;
jack1930 0:d7ad0916857f 116 schwenker=0.025+(ws/180.0)*0.1;
jack1930 0:d7ad0916857f 117 dreher=0.025+(wd/180.0)*0.1;
jack1930 0:d7ad0916857f 118 HAL_Delay(20);
jack1930 0:d7ad0916857f 119 }
jack1930 0:d7ad0916857f 120
jack1930 0:d7ad0916857f 121 wg0=wg1;
jack1930 0:d7ad0916857f 122 wh0=wh1;
jack1930 0:d7ad0916857f 123 ws0=ws1;
jack1930 0:d7ad0916857f 124 wd0=wd1;
jack1930 0:d7ad0916857f 125
jack1930 0:d7ad0916857f 126 mylcd.cursorpos(0);
jack1930 0:d7ad0916857f 127 mylcd.printf("Schritt 3");
jack1930 0:d7ad0916857f 128
jack1930 0:d7ad0916857f 129 wg1=180;
jack1930 0:d7ad0916857f 130 wh1=60;
jack1930 0:d7ad0916857f 131 ws1=100;
jack1930 0:d7ad0916857f 132 wd1=150;
jack1930 0:d7ad0916857f 133 ag=(wg1-wg0)/schritte;
jack1930 0:d7ad0916857f 134 ah=(wh1-wh0)/schritte;
jack1930 0:d7ad0916857f 135 as=(ws1-ws0)/schritte;
jack1930 0:d7ad0916857f 136 ad=(wd1-wd0)/schritte;
jack1930 0:d7ad0916857f 137 for (float i=0;i<schritte;i++)
jack1930 0:d7ad0916857f 138 {
jack1930 0:d7ad0916857f 139 wg=ag*i+wg0;
jack1930 0:d7ad0916857f 140 wh=ah*i+wh0;
jack1930 0:d7ad0916857f 141 ws=as*i+ws0;
jack1930 0:d7ad0916857f 142 wd=ad*i+wd0;
jack1930 0:d7ad0916857f 143 greifer=0.025+(wg/180.0)*0.1;
jack1930 0:d7ad0916857f 144 heber=0.025+(wh/180.0)*0.1;
jack1930 0:d7ad0916857f 145 schwenker=0.025+(ws/180.0)*0.1;
jack1930 0:d7ad0916857f 146 dreher=0.025+(wd/180.0)*0.1;
jack1930 0:d7ad0916857f 147 HAL_Delay(20);
jack1930 0:d7ad0916857f 148 }
jack1930 0:d7ad0916857f 149
jack1930 0:d7ad0916857f 150 wg0=wg1;
jack1930 0:d7ad0916857f 151 wh0=wh1;
jack1930 0:d7ad0916857f 152 ws0=ws1;
jack1930 0:d7ad0916857f 153 wd0=wd1;
jack1930 0:d7ad0916857f 154
jack1930 0:d7ad0916857f 155 mylcd.cursorpos(0);
jack1930 0:d7ad0916857f 156 mylcd.printf("Schritt 4");
jack1930 0:d7ad0916857f 157
jack1930 0:d7ad0916857f 158 wg1=180;
jack1930 0:d7ad0916857f 159 wh1=30;
jack1930 0:d7ad0916857f 160 ws1=100;
jack1930 0:d7ad0916857f 161 wd1=30;
jack1930 0:d7ad0916857f 162 ag=(wg1-wg0)/schritte;
jack1930 0:d7ad0916857f 163 ah=(wh1-wh0)/schritte;
jack1930 0:d7ad0916857f 164 as=(ws1-ws0)/schritte;
jack1930 0:d7ad0916857f 165 ad=(wd1-wd0)/schritte;
jack1930 0:d7ad0916857f 166 for (float i=0;i<schritte;i++)
jack1930 0:d7ad0916857f 167 {
jack1930 0:d7ad0916857f 168 wg=ag*i+wg0;
jack1930 0:d7ad0916857f 169 wh=ah*i+wh0;
jack1930 0:d7ad0916857f 170 ws=as*i+ws0;
jack1930 0:d7ad0916857f 171 wd=ad*i+wd0;
jack1930 0:d7ad0916857f 172 greifer=0.025+(wg/180.0)*0.1;
jack1930 0:d7ad0916857f 173 heber=0.025+(wh/180.0)*0.1;
jack1930 0:d7ad0916857f 174 schwenker=0.025+(ws/180.0)*0.1;
jack1930 0:d7ad0916857f 175 dreher=0.025+(wd/180.0)*0.1;
jack1930 0:d7ad0916857f 176 HAL_Delay(20);
jack1930 0:d7ad0916857f 177 }
jack1930 0:d7ad0916857f 178
jack1930 0:d7ad0916857f 179 wg0=wg1;
jack1930 0:d7ad0916857f 180 wh0=wh1;
jack1930 0:d7ad0916857f 181 ws0=ws1;
jack1930 0:d7ad0916857f 182 wd0=wd1;
jack1930 0:d7ad0916857f 183
jack1930 0:d7ad0916857f 184 mylcd.cursorpos(0);
jack1930 0:d7ad0916857f 185 mylcd.printf("Schritt 5");
jack1930 0:d7ad0916857f 186
jack1930 0:d7ad0916857f 187 wg1=120;
jack1930 0:d7ad0916857f 188 wh1=30;
jack1930 0:d7ad0916857f 189 ws1=100;
jack1930 0:d7ad0916857f 190 wd1=30;
jack1930 0:d7ad0916857f 191 ag=(wg1-wg0)/schritte;
jack1930 0:d7ad0916857f 192 ah=(wh1-wh0)/schritte;
jack1930 0:d7ad0916857f 193 as=(ws1-ws0)/schritte;
jack1930 0:d7ad0916857f 194 ad=(wd1-wd0)/schritte;
jack1930 0:d7ad0916857f 195 for (float i=0;i<schritte;i++)
jack1930 0:d7ad0916857f 196 {
jack1930 0:d7ad0916857f 197 wg=ag*i+wg0;
jack1930 0:d7ad0916857f 198 wh=ah*i+wh0;
jack1930 0:d7ad0916857f 199 ws=as*i+ws0;
jack1930 0:d7ad0916857f 200 wd=ad*i+wd0;
jack1930 0:d7ad0916857f 201 greifer=0.025+(wg/180.0)*0.1;
jack1930 0:d7ad0916857f 202 heber=0.025+(wh/180.0)*0.1;
jack1930 0:d7ad0916857f 203 schwenker=0.025+(ws/180.0)*0.1;
jack1930 0:d7ad0916857f 204 dreher=0.025+(wd/180.0)*0.1;
jack1930 0:d7ad0916857f 205 HAL_Delay(20);
jack1930 0:d7ad0916857f 206 }
jack1930 0:d7ad0916857f 207
jack1930 0:d7ad0916857f 208 wg0=wg1;
jack1930 0:d7ad0916857f 209 wh0=wh1;
jack1930 0:d7ad0916857f 210 ws0=ws1;
jack1930 0:d7ad0916857f 211 wd0=wd1;
jack1930 0:d7ad0916857f 212
jack1930 0:d7ad0916857f 213 mylcd.cursorpos(0);
jack1930 0:d7ad0916857f 214 mylcd.printf("Schritt 6");
jack1930 0:d7ad0916857f 215
jack1930 0:d7ad0916857f 216 wg1=100;
jack1930 0:d7ad0916857f 217 wh1=60;
jack1930 0:d7ad0916857f 218 ws1=100;
jack1930 0:d7ad0916857f 219 wd1=30;
jack1930 0:d7ad0916857f 220 ag=(wg1-wg0)/schritte;
jack1930 0:d7ad0916857f 221 ah=(wh1-wh0)/schritte;
jack1930 0:d7ad0916857f 222 as=(ws1-ws0)/schritte;
jack1930 0:d7ad0916857f 223 ad=(wd1-wd0)/schritte;
jack1930 0:d7ad0916857f 224 for (float i=0;i<schritte;i++)
jack1930 0:d7ad0916857f 225 {
jack1930 0:d7ad0916857f 226 wg=ag*i+wg0;
jack1930 0:d7ad0916857f 227 wh=ah*i+wh0;
jack1930 0:d7ad0916857f 228 ws=as*i+ws0;
jack1930 0:d7ad0916857f 229 wd=ad*i+wd0;
jack1930 0:d7ad0916857f 230 greifer=0.025+(wg/180.0)*0.1;
jack1930 0:d7ad0916857f 231 heber=0.025+(wh/180.0)*0.1;
jack1930 0:d7ad0916857f 232 schwenker=0.025+(ws/180.0)*0.1;
jack1930 0:d7ad0916857f 233 dreher=0.025+(wd/180.0)*0.1;
jack1930 0:d7ad0916857f 234 HAL_Delay(20);
jack1930 0:d7ad0916857f 235 }
jack1930 0:d7ad0916857f 236
jack1930 0:d7ad0916857f 237 wg0=wg1;
jack1930 0:d7ad0916857f 238 wh0=wh1;
jack1930 0:d7ad0916857f 239 ws0=ws1;
jack1930 0:d7ad0916857f 240 wd0=wd1;
jack1930 0:d7ad0916857f 241 }
jack1930 0:d7ad0916857f 242 }