Antonia Baumgartner / Mbed 2 deprecated YB_copy

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of Versuch21 by Antonia Baumgartner

Classes/Motion.cpp@4:3c6d2c035243, 2018-05-01 (annotated)

Committer:
baumgant
Date:
Tue May 01 11:42:38 2018 +0000
Revision:
4:3c6d2c035243
Parent:
3:f44ef28cfb2d
Child:
5:93d3efe46493
PES2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
baumgant 0:b886f13e4ac6 1 #include "Motion.h"
baumgant 0:b886f13e4ac6 2 #include "Controller.h"
baumgant 0:b886f13e4ac6 3 #include "EncoderCounter.h"
baumgant 0:b886f13e4ac6 4 #include "Spurhaltung.h"
mrbb2 1:6ef5bc60e69c 5 #include "IRSensorGF.h"
mrbb2 1:6ef5bc60e69c 6 #include "IRSensorG.h"
mrbb2 1:6ef5bc60e69c 7 #include "IRSensorK.h"
mrbb2 1:6ef5bc60e69c 8 #include "IRSensorZ.h"
baumgant 0:b886f13e4ac6 9
mrbb2 3:f44ef28cfb2d 10 // Left + = Rückwärts Left - = Vorwärts
baumgant 2:efa9a78591da 11 // Right + = Vorwärts Right - = Rückwärts
baumgant 0:b886f13e4ac6 12
baumgant 0:b886f13e4ac6 13 using namespace std;
baumgant 0:b886f13e4ac6 14
mrbb2 1:6ef5bc60e69c 15 Motion::Motion(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, Spurhaltung& spurhaltung, IRSensorGF& Sensor1, IRSensorG& Sensor2, IRSensorG& Sensor3, IRSensorK& Sensor4, IRSensorK& Sensor5, IRSensorZ& Sensor6):
mrbb2 1:6ef5bc60e69c 16 counterLeft(counterLeft), counterRight(counterRight), controller(controller), spurhaltung(spurhaltung), Sensor1(Sensor1), Sensor2(Sensor2), Sensor3(Sensor3), Sensor4(Sensor4), Sensor5(Sensor5), Sensor6(Sensor6)
baumgant 0:b886f13e4ac6 17 {
mrbb2 3:f44ef28cfb2d 18
baumgant 0:b886f13e4ac6 19 }
baumgant 0:b886f13e4ac6 20
baumgant 0:b886f13e4ac6 21 Motion::~Motion() {}
baumgant 0:b886f13e4ac6 22
baumgant 2:efa9a78591da 23 //------------------------------------------------------------------------------
mrbb2 1:6ef5bc60e69c 24
mrbb2 1:6ef5bc60e69c 25 void Motion::drehenl90()
mrbb2 1:6ef5bc60e69c 26 {
mrbb2 1:6ef5bc60e69c 27 controller.resetCounter();
mrbb2 1:6ef5bc60e69c 28 int lastCountRight = counterRight.read();
mrbb2 1:6ef5bc60e69c 29
baumgant 4:3c6d2c035243 30 while(counterRight.read()-lastCountRight>(-1.19*1260)) {
baumgant 2:efa9a78591da 31 controller.setDesiredSpeedLeft(150.0f); //Drehzahl in [rpm]
baumgant 2:efa9a78591da 32 controller.setDesiredSpeedRight(150.0f);
mrbb2 1:6ef5bc60e69c 33 }
mrbb2 1:6ef5bc60e69c 34 controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
mrbb2 1:6ef5bc60e69c 35 controller.setDesiredSpeedRight(0.0f);
mrbb2 1:6ef5bc60e69c 36
mrbb2 1:6ef5bc60e69c 37 }
mrbb2 1:6ef5bc60e69c 38
baumgant 2:efa9a78591da 39 //------------------------------------------------------------------------------
baumgant 2:efa9a78591da 40
baumgant 2:efa9a78591da 41 void Motion::gerade()
baumgant 2:efa9a78591da 42 {
baumgant 2:efa9a78591da 43
baumgant 2:efa9a78591da 44 controller.resetCounter();
baumgant 2:efa9a78591da 45 int lastCountLeft = counterLeft.read();
mrbb2 3:f44ef28cfb2d 46
mrbb2 3:f44ef28cfb2d 47 if (Sensor1.read() == 0) {
baumgant 4:3c6d2c035243 48 while(counterLeft.read()-lastCountLeft>(-2.45*1260)) {
mrbb2 3:f44ef28cfb2d 49 spurhaltung.speedl();
mrbb2 3:f44ef28cfb2d 50 spurhaltung.speedr();
mrbb2 3:f44ef28cfb2d 51 controller.setDesiredSpeedLeft(-spurhaltung.speedr()); //Drehzahl in [rpm]
mrbb2 3:f44ef28cfb2d 52 controller.setDesiredSpeedRight(spurhaltung.speedl());
mrbb2 3:f44ef28cfb2d 53 }
baumgant 2:efa9a78591da 54 }
baumgant 2:efa9a78591da 55 controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
baumgant 2:efa9a78591da 56 controller.setDesiredSpeedRight(0.0f);
baumgant 2:efa9a78591da 57 }
baumgant 2:efa9a78591da 58
baumgant 2:efa9a78591da 59 //------------------------------------------------------------------------------
baumgant 2:efa9a78591da 60
mrbb2 1:6ef5bc60e69c 61 void Motion::drehenr90()
mrbb2 1:6ef5bc60e69c 62 {
mrbb2 1:6ef5bc60e69c 63 controller.resetCounter();
mrbb2 1:6ef5bc60e69c 64 int lastCountLeft = counterLeft.read();
mrbb2 1:6ef5bc60e69c 65
baumgant 4:3c6d2c035243 66 while(counterLeft.read()-lastCountLeft<(1.19*1260)) {
baumgant 2:efa9a78591da 67 controller.setDesiredSpeedLeft(-150.0f); //Drehzahl in [rpm]
baumgant 2:efa9a78591da 68 controller.setDesiredSpeedRight(-150.0f);
mrbb2 1:6ef5bc60e69c 69 }
mrbb2 1:6ef5bc60e69c 70 controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
mrbb2 1:6ef5bc60e69c 71 controller.setDesiredSpeedRight(0.0f);
mrbb2 1:6ef5bc60e69c 72
mrbb2 1:6ef5bc60e69c 73 }
mrbb2 1:6ef5bc60e69c 74
baumgant 2:efa9a78591da 75 //------------------------------------------------------------------------------
baumgant 2:efa9a78591da 76
mrbb2 1:6ef5bc60e69c 77 void Motion::drehen180()
mrbb2 1:6ef5bc60e69c 78 {
mrbb2 1:6ef5bc60e69c 79 controller.resetCounter();
mrbb2 1:6ef5bc60e69c 80 int lastCountRight = counterRight.read();
mrbb2 1:6ef5bc60e69c 81
baumgant 4:3c6d2c035243 82 while(counterRight.read()-lastCountRight>(-2.38*1260)) {
mrbb2 1:6ef5bc60e69c 83 controller.setDesiredSpeedLeft(150.0f); //Drehzahl in [rpm]
mrbb2 1:6ef5bc60e69c 84 controller.setDesiredSpeedRight(150.0f);
mrbb2 1:6ef5bc60e69c 85 }
mrbb2 1:6ef5bc60e69c 86 controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
mrbb2 1:6ef5bc60e69c 87 controller.setDesiredSpeedRight(0.0f);
mrbb2 1:6ef5bc60e69c 88 }
baumgant 0:b886f13e4ac6 89
baumgant 2:efa9a78591da 90 //------------------------------------------------------------------------------
baumgant 2:efa9a78591da 91
baumgant 0:b886f13e4ac6 92 void Motion::switching(int D)
baumgant 0:b886f13e4ac6 93 {
baumgant 0:b886f13e4ac6 94 switch(D) {
baumgant 0:b886f13e4ac6 95 case 1:
mrbb2 1:6ef5bc60e69c 96 // Left
mrbb2 1:6ef5bc60e69c 97 drehenl90();
baumgant 0:b886f13e4ac6 98 // Forward
mrbb2 3:f44ef28cfb2d 99 gerade();
baumgant 0:b886f13e4ac6 100 break;
baumgant 0:b886f13e4ac6 101
baumgant 0:b886f13e4ac6 102 case 2:
mrbb2 1:6ef5bc60e69c 103 // Forward
mrbb2 1:6ef5bc60e69c 104 gerade();
baumgant 0:b886f13e4ac6 105 break;
baumgant 0:b886f13e4ac6 106
baumgant 0:b886f13e4ac6 107 case 3:
mrbb2 1:6ef5bc60e69c 108 // Right
mrbb2 1:6ef5bc60e69c 109 drehenr90();
baumgant 0:b886f13e4ac6 110 // Forward
mrbb2 1:6ef5bc60e69c 111 gerade();
baumgant 0:b886f13e4ac6 112 break;
baumgant 0:b886f13e4ac6 113
mrbb2 1:6ef5bc60e69c 114 case 4:
mrbb2 1:6ef5bc60e69c 115 // 180 Drehen
mrbb2 1:6ef5bc60e69c 116 drehen180();
mrbb2 1:6ef5bc60e69c 117 break;
baumgant 0:b886f13e4ac6 118
baumgant 0:b886f13e4ac6 119 default:
baumgant 0:b886f13e4ac6 120 controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
baumgant 0:b886f13e4ac6 121 controller.setDesiredSpeedRight(0.0f);
baumgant 0:b886f13e4ac6 122 }
mrbb2 3:f44ef28cfb2d 123
baumgant 2:efa9a78591da 124 //------------------------------------------------------------------------------
mrbb2 1:6ef5bc60e69c 125
mrbb2 3:f44ef28cfb2d 126 /*void Motion::switching()
mrbb2 3:f44ef28cfb2d 127 {
mrbb2 3:f44ef28cfb2d 128 if(Sensor4.read() == 0) {
mrbb2 3:f44ef28cfb2d 129 // Left
mrbb2 3:f44ef28cfb2d 130 drehenl90();
mrbb2 3:f44ef28cfb2d 131 // Forward
mrbb2 3:f44ef28cfb2d 132 gerade();
mrbb2 3:f44ef28cfb2d 133 } else if (Sensor1.read() == 0) {
mrbb2 3:f44ef28cfb2d 134 // Forward
mrbb2 3:f44ef28cfb2d 135 gerade();
mrbb2 3:f44ef28cfb2d 136 } else if (Sensor5.read() == 0) {
mrbb2 3:f44ef28cfb2d 137 // Right
mrbb2 3:f44ef28cfb2d 138 drehenr90();
mrbb2 3:f44ef28cfb2d 139 // Forward
mrbb2 3:f44ef28cfb2d 140 gerade();
mrbb2 3:f44ef28cfb2d 141 } else {
mrbb2 3:f44ef28cfb2d 142 // 180 Drehen
mrbb2 3:f44ef28cfb2d 143 drehen180();
mrbb2 3:f44ef28cfb2d 144 // Forward
mrbb2 3:f44ef28cfb2d 145 gerade();
mrbb2 3:f44ef28cfb2d 146
mrbb2 3:f44ef28cfb2d 147 }*/
baumgant 0:b886f13e4ac6 148
baumgant 0:b886f13e4ac6 149 }