Yuri T / Mbed 2 deprecated miniRobot

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

main.cpp@2:d8db9db36fb7, 2014-10-17 (annotated)

Committer:
geovas
Date:
Fri Oct 17 15:24:58 2014 +0000
Revision:
2:d8db9db36fb7
Parent:
1:29eeaabaa8e1
Child:
3:6ec54d838b44
bug fix

Who changed what in which revision?

UserRevisionLine numberNew contents of line
geovas 0:341347d8ccbc 1 #include "mbed.h"
geovas 2:d8db9db36fb7 2 #include "echo_sensor.h" // echo sensor library
geovas 0:341347d8ccbc 3
geovas 0:341347d8ccbc 4 #define LM1_ D3
geovas 1:29eeaabaa8e1 5 #define LM2_ D7
geovas 0:341347d8ccbc 6 #define RM1_ D6
geovas 1:29eeaabaa8e1 7 #define RM2_ D5
geovas 0:341347d8ccbc 8 #define RX_ D0
geovas 0:341347d8ccbc 9 #define TX_ D1
geovas 0:341347d8ccbc 10
geovas 0:341347d8ccbc 11 PwmOut LM1(LM1_); // motors configuration
geovas 0:341347d8ccbc 12 PwmOut LM2(LM2_);
geovas 0:341347d8ccbc 13 PwmOut RM1(RM1_);
geovas 0:341347d8ccbc 14 PwmOut RM2(LM2_);
geovas 0:341347d8ccbc 15
geovas 1:29eeaabaa8e1 16 //Serial link(TX_, RX_); // bluetooth serial port
geovas 0:341347d8ccbc 17 Serial link(USBTX, USBRX); // usb serial port
geovas 0:341347d8ccbc 18
geovas 0:341347d8ccbc 19 void analogWrite(mbed::PwmOut *prt, char spd) // write to PWM
geovas 0:341347d8ccbc 20 {
geovas 0:341347d8ccbc 21 float a = spd / 2.56; // convert 0..256 to '%'
geovas 0:341347d8ccbc 22 *prt = a;
geovas 0:341347d8ccbc 23 }
geovas 0:341347d8ccbc 24
geovas 0:341347d8ccbc 25 void GoForward(char spd)
geovas 0:341347d8ccbc 26 {
geovas 0:341347d8ccbc 27 analogWrite(&LM1,spd);
geovas 0:341347d8ccbc 28 analogWrite(&LM2,0);
geovas 0:341347d8ccbc 29 analogWrite(&RM1,spd);
geovas 0:341347d8ccbc 30 analogWrite(&RM2,0);
geovas 0:341347d8ccbc 31 }
geovas 0:341347d8ccbc 32
geovas 0:341347d8ccbc 33 void GoBack(char spd)
geovas 0:341347d8ccbc 34 {
geovas 0:341347d8ccbc 35 analogWrite(&LM1,0);
geovas 0:341347d8ccbc 36 analogWrite(&LM2,spd);
geovas 0:341347d8ccbc 37 analogWrite(&RM1,0);
geovas 0:341347d8ccbc 38 analogWrite(&RM2,spd);
geovas 0:341347d8ccbc 39 }
geovas 0:341347d8ccbc 40
geovas 0:341347d8ccbc 41 void TurnRight(char spd)
geovas 0:341347d8ccbc 42 {
geovas 0:341347d8ccbc 43 analogWrite(&LM1,spd);
geovas 0:341347d8ccbc 44 analogWrite(&LM2,0);
geovas 0:341347d8ccbc 45 analogWrite(&RM1,0);
geovas 0:341347d8ccbc 46 analogWrite(&RM2,spd);
geovas 0:341347d8ccbc 47 }
geovas 0:341347d8ccbc 48
geovas 0:341347d8ccbc 49 void TurnLeft(char spd)
geovas 0:341347d8ccbc 50 {
geovas 0:341347d8ccbc 51
geovas 0:341347d8ccbc 52 analogWrite(&LM1,0);
geovas 0:341347d8ccbc 53 analogWrite(&LM2,spd);
geovas 0:341347d8ccbc 54 analogWrite(&RM1,spd);
geovas 0:341347d8ccbc 55 analogWrite(&RM2,0);
geovas 0:341347d8ccbc 56 }
geovas 0:341347d8ccbc 57
geovas 0:341347d8ccbc 58 int charToInt(char a, char b, char c)
geovas 0:341347d8ccbc 59 {
geovas 0:341347d8ccbc 60 int ans;
geovas 0:341347d8ccbc 61 if (a>'9' || b>'9' || c>'9') return 0;
geovas 0:341347d8ccbc 62 if (a<'0' || b<'0' || c<'0') return 0;
geovas 0:341347d8ccbc 63 ans = (a - 48)*100 + (b - 48)*10 + (c - 48);
geovas 0:341347d8ccbc 64 if (ans > 255) ans = 255;
geovas 0:341347d8ccbc 65 return ans;
geovas 0:341347d8ccbc 66 }
geovas 0:341347d8ccbc 67
geovas 0:341347d8ccbc 68 char str[4];
geovas 0:341347d8ccbc 69 void intToChar(int dat)
geovas 0:341347d8ccbc 70 {
geovas 0:341347d8ccbc 71 int d = dat;
geovas 0:341347d8ccbc 72
geovas 0:341347d8ccbc 73 if (dat<100) str[0] = '0';
geovas 0:341347d8ccbc 74 else
geovas 0:341347d8ccbc 75 {
geovas 0:341347d8ccbc 76 str[0] = d/100 + 48;
geovas 0:341347d8ccbc 77 d %= 100;
geovas 0:341347d8ccbc 78 }
geovas 0:341347d8ccbc 79 if (dat<10) str[1] = '0';
geovas 0:341347d8ccbc 80 else
geovas 0:341347d8ccbc 81 {
geovas 0:341347d8ccbc 82 str[1] = d/10 + 48;
geovas 0:341347d8ccbc 83 d %= 10;
geovas 0:341347d8ccbc 84 }
geovas 0:341347d8ccbc 85 str[2] = d + 48;
geovas 0:341347d8ccbc 86 }
geovas 0:341347d8ccbc 87
geovas 0:341347d8ccbc 88 char toupper(char ch)
geovas 0:341347d8ccbc 89 {
geovas 0:341347d8ccbc 90 if (ch >= 'a' && ch <= 'z') return ch;
geovas 0:341347d8ccbc 91 if (ch >= 'A' && ch <= 'Z') return ch - 'A' + 'a';
geovas 0:341347d8ccbc 92 return 0;
geovas 0:341347d8ccbc 93 }
geovas 0:341347d8ccbc 94
geovas 0:341347d8ccbc 95 int main(void)
geovas 0:341347d8ccbc 96 {
geovas 0:341347d8ccbc 97 char dat[5], b, i;
geovas 0:341347d8ccbc 98 link.baud(9600);
geovas 1:29eeaabaa8e1 99 initEcho(); // initialize echo sensor
geovas 0:341347d8ccbc 100 GoForward(0); // stop all motors
geovas 0:341347d8ccbc 101
geovas 2:d8db9db36fb7 102 while (!link.readable()); // trying to fount the starting byte
geovas 0:341347d8ccbc 103 while (link.getc() != 'Z');
geovas 0:341347d8ccbc 104
geovas 0:341347d8ccbc 105 while (true)
geovas 0:341347d8ccbc 106 {
geovas 0:341347d8ccbc 107 if(link.readable()) // read data from blutooth serial port
geovas 0:341347d8ccbc 108 {
geovas 0:341347d8ccbc 109 for (i=0; i<5; i++)
geovas 0:341347d8ccbc 110 dat[i] = link.getc();
geovas 0:341347d8ccbc 111 }
geovas 0:341347d8ccbc 112
geovas 0:341347d8ccbc 113 b = toupper(dat[0]);
geovas 0:341347d8ccbc 114
geovas 0:341347d8ccbc 115 int spd = charToInt(dat[1],dat[2],dat[3]);
geovas 0:341347d8ccbc 116
geovas 1:29eeaabaa8e1 117 if (b == 'F') GoForward(spd); // movements
geovas 0:341347d8ccbc 118 if (b == 'B') GoBack(spd);
geovas 0:341347d8ccbc 119 if (b == 'R') TurnRight(spd);
geovas 0:341347d8ccbc 120 if (b == 'L') TurnLeft(spd);
geovas 0:341347d8ccbc 121 if (b == 'N') GoForward(0);
geovas 0:341347d8ccbc 122
geovas 0:341347d8ccbc 123 if (b == 'E') // echo sensor
geovas 0:341347d8ccbc 124 {
geovas 1:29eeaabaa8e1 125 intToChar(getEcho());
geovas 1:29eeaabaa8e1 126 link.printf(str);
geovas 0:341347d8ccbc 127 }
geovas 0:341347d8ccbc 128 }
geovas 0:341347d8ccbc 129 }