project enzo / Mbed 2 deprecated robotV1_0

jh

Dependencies:   mbed

main.cpp@0:70d06581738c, 2017-06-02 (annotated)

Committer:
joosthartkamp
Date:
Fri Jun 02 16:44:01 2017 +0000
Revision:
0:70d06581738c
hghfg

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joosthartkamp 0:70d06581738c 1 #include "mbed.h"
joosthartkamp 0:70d06581738c 2
joosthartkamp 0:70d06581738c 3 Serial bt(USBTX, USBRX);
joosthartkamp 0:70d06581738c 4
joosthartkamp 0:70d06581738c 5 AnalogIn linksvoor(A0);
joosthartkamp 0:70d06581738c 6 AnalogIn rechtsvoor(A1);
joosthartkamp 0:70d06581738c 7 AnalogIn linksachter(A2);
joosthartkamp 0:70d06581738c 8 AnalogIn rechtsachter(A3);
joosthartkamp 0:70d06581738c 9 AnalogIn afstand(A4);
joosthartkamp 0:70d06581738c 10
joosthartkamp 0:70d06581738c 11 DigitalOut steppulse(D9);
joosthartkamp 0:70d06581738c 12 DigitalOut enable(D10);
joosthartkamp 0:70d06581738c 13 DigitalIn button(D8);
joosthartkamp 0:70d06581738c 14 DigitalOut led(D11);
joosthartkamp 0:70d06581738c 15
joosthartkamp 0:70d06581738c 16 PwmOut motorlfwd(D2);
joosthartkamp 0:70d06581738c 17 PwmOut motorlrwd(D3);
joosthartkamp 0:70d06581738c 18 PwmOut motorrfwd(D6);
joosthartkamp 0:70d06581738c 19 PwmOut motorrrwd(D7);
joosthartkamp 0:70d06581738c 20
joosthartkamp 0:70d06581738c 21 Ticker stepperspeed;
joosthartkamp 0:70d06581738c 22 Ticker stepperspeed;
joosthartkamp 0:70d06581738c 23
joosthartkamp 0:70d06581738c 24
joosthartkamp 0:70d06581738c 25 int pos;
joosthartkamp 0:70d06581738c 26 bool robotrun;
joosthartkamp 0:70d06581738c 27 bool object;
joosthartkamp 0:70d06581738c 28 int radarspeedsetleft;
joosthartkamp 0:70d06581738c 29 int radarspeedsetright;
joosthartkamp 0:70d06581738c 30 bool lijnfl;
joosthartkamp 0:70d06581738c 31 bool lijnfr;
joosthartkamp 0:70d06581738c 32 bool lijnrl;
joosthartkamp 0:70d06581738c 33 bool lijnrr;
joosthartkamp 0:70d06581738c 34
joosthartkamp 0:70d06581738c 35 void Hbrug(float left, float right)
joosthartkamp 0:70d06581738c 36 {
joosthartkamp 0:70d06581738c 37 if (left > 0) {
joosthartkamp 0:70d06581738c 38 motorlrwd = 0;
joosthartkamp 0:70d06581738c 39 motorlfwd = left;
joosthartkamp 0:70d06581738c 40 } else if(left < 0) {
joosthartkamp 0:70d06581738c 41 motorlfwd = 0;
joosthartkamp 0:70d06581738c 42 motorlrwd = left*-1;
joosthartkamp 0:70d06581738c 43 } else {
joosthartkamp 0:70d06581738c 44 motorlrwd = 0;
joosthartkamp 0:70d06581738c 45 motorlfwd = 0;
joosthartkamp 0:70d06581738c 46 }
joosthartkamp 0:70d06581738c 47 if (right > 0) {
joosthartkamp 0:70d06581738c 48 motorrrwd = 0;
joosthartkamp 0:70d06581738c 49 motorrfwd = right;
joosthartkamp 0:70d06581738c 50 } else if(right < 0) {
joosthartkamp 0:70d06581738c 51 motorrfwd = 0;
joosthartkamp 0:70d06581738c 52 motorrrwd = right*-1;
joosthartkamp 0:70d06581738c 53 } else {
joosthartkamp 0:70d06581738c 54 motorrrwd = 0;
joosthartkamp 0:70d06581738c 55 motorrfwd = 0;
joosthartkamp 0:70d06581738c 56 }
joosthartkamp 0:70d06581738c 57 }
joosthartkamp 0:70d06581738c 58 void radar()
joosthartkamp 0:70d06581738c 59 {
joosthartkamp 0:70d06581738c 60 float distance;
joosthartkamp 0:70d06581738c 61 distance = afstand;
joosthartkamp 0:70d06581738c 62 if (distance < 0.2) {
joosthartkamp 0:70d06581738c 63 object = 1;
joosthartkamp 0:70d06581738c 64 } else {
joosthartkamp 0:70d06581738c 65 object = 0;
joosthartkamp 0:70d06581738c 66 }
joosthartkamp 0:70d06581738c 67 }
joosthartkamp 0:70d06581738c 68 void stepper()
joosthartkamp 0:70d06581738c 69 {
joosthartkamp 0:70d06581738c 70 if (robotrun == 1) {
joosthartkamp 0:70d06581738c 71 steppulse = !steppulse;
joosthartkamp 0:70d06581738c 72 pos ++;
joosthartkamp 0:70d06581738c 73 if (pos>2080) {
joosthartkamp 0:70d06581738c 74 pos = 0;
joosthartkamp 0:70d06581738c 75 }
joosthartkamp 0:70d06581738c 76 }
joosthartkamp 0:70d06581738c 77 }
joosthartkamp 0:70d06581738c 78 void lijnsensor()
joosthartkamp 0:70d06581738c 79 {
joosthartkamp 0:70d06581738c 80 float a= linksvoor;
joosthartkamp 0:70d06581738c 81 float b= rechtsvoor;
joosthartkamp 0:70d06581738c 82 float c= linksachter;
joosthartkamp 0:70d06581738c 83 float d= rechtsachter;
joosthartkamp 0:70d06581738c 84 if (a < 0.5) {
joosthartkamp 0:70d06581738c 85 lijnfl = 1;
joosthartkamp 0:70d06581738c 86 } else {
joosthartkamp 0:70d06581738c 87 lijnfl = 0;
joosthartkamp 0:70d06581738c 88 }
joosthartkamp 0:70d06581738c 89 if (b < 0.5) {
joosthartkamp 0:70d06581738c 90 lijnfl = 1;
joosthartkamp 0:70d06581738c 91 } else {
joosthartkamp 0:70d06581738c 92 lijnfl = 0;
joosthartkamp 0:70d06581738c 93 }
joosthartkamp 0:70d06581738c 94 if (c < 0.5) {
joosthartkamp 0:70d06581738c 95 lijnfl = 1;
joosthartkamp 0:70d06581738c 96 } else {
joosthartkamp 0:70d06581738c 97 lijnfl = 0;
joosthartkamp 0:70d06581738c 98 }
joosthartkamp 0:70d06581738c 99 if (d < 0.5) {
joosthartkamp 0:70d06581738c 100 lijnfl = 1;
joosthartkamp 0:70d06581738c 101 } else {
joosthartkamp 0:70d06581738c 102 lijnfl = 0;
joosthartkamp 0:70d06581738c 103 }
joosthartkamp 0:70d06581738c 104 }
joosthartkamp 0:70d06581738c 105 int main ()
joosthartkamp 0:70d06581738c 106 {
joosthartkamp 0:70d06581738c 107 enable = 0;
joosthartkamp 0:70d06581738c 108 while (1) {
joosthartkamp 0:70d06581738c 109 if (robotrun == 1){
joosthartkamp 0:70d06581738c 110 if (lijnfl ==1){
joosthartkamp 0:70d06581738c 111 Hbrug(-1,-0.5);
joosthartkamp 0:70d06581738c 112 }
joosthartkamp 0:70d06581738c 113 else if (lijnfl ==1){
joosthartkamp 0:70d06581738c 114 Hbrug(-1,-0.5);
joosthartkamp 0:70d06581738c 115 }
joosthartkamp 0:70d06581738c 116 }
joosthartkamp 0:70d06581738c 117 else {
joosthartkamp 0:70d06581738c 118 Hbrug(0,0);
joosthartkamp 0:70d06581738c 119 }
joosthartkamp 0:70d06581738c 120 }
joosthartkamp 0:70d06581738c 121 }