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@0:341347d8ccbc, 2014-10-15 (annotated)

Committer:
geovas
Date:
Wed Oct 15 15:09:43 2014 +0000
Revision:
0:341347d8ccbc
Child:
1:29eeaabaa8e1
miniRobot firmware v1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
geovas 0:341347d8ccbc 1 #include "mbed.h"
geovas 0:341347d8ccbc 2
geovas 0:341347d8ccbc 3 #define trigPin_ PTC11
geovas 0:341347d8ccbc 4 #define echoPin_ PTC12
geovas 0:341347d8ccbc 5 #define volt_Pin_ PTB11
geovas 0:341347d8ccbc 6 #define LM1_ D3
geovas 0:341347d8ccbc 7 #define LM2_ D5
geovas 0:341347d8ccbc 8 #define RM1_ D6
geovas 0:341347d8ccbc 9 #define RM2_ D7
geovas 0:341347d8ccbc 10 #define RX_ D0
geovas 0:341347d8ccbc 11 #define TX_ D1
geovas 0:341347d8ccbc 12 #define SPEED_ 64
geovas 0:341347d8ccbc 13
geovas 0:341347d8ccbc 14 PwmOut LM1(LM1_); // motors configuration
geovas 0:341347d8ccbc 15 PwmOut LM2(LM2_);
geovas 0:341347d8ccbc 16 PwmOut RM1(RM1_);
geovas 0:341347d8ccbc 17 PwmOut RM2(LM2_);
geovas 0:341347d8ccbc 18
geovas 0:341347d8ccbc 19 //Serial link(TX_, RX_); // bluetooth serial port
geovas 0:341347d8ccbc 20 Serial link(USBTX, USBRX); // usb serial port
geovas 0:341347d8ccbc 21
geovas 0:341347d8ccbc 22 void analogWrite(mbed::PwmOut *prt, char spd) // write to PWM
geovas 0:341347d8ccbc 23 {
geovas 0:341347d8ccbc 24 float a = spd / 2.56; // convert 0..256 to '%'
geovas 0:341347d8ccbc 25 *prt = a;
geovas 0:341347d8ccbc 26 }
geovas 0:341347d8ccbc 27
geovas 0:341347d8ccbc 28 void GoForward(char spd)
geovas 0:341347d8ccbc 29 {
geovas 0:341347d8ccbc 30 analogWrite(&LM1,spd);
geovas 0:341347d8ccbc 31 analogWrite(&LM2,0);
geovas 0:341347d8ccbc 32 analogWrite(&RM1,spd);
geovas 0:341347d8ccbc 33 analogWrite(&RM2,0);
geovas 0:341347d8ccbc 34 }
geovas 0:341347d8ccbc 35
geovas 0:341347d8ccbc 36 void GoBack(char spd)
geovas 0:341347d8ccbc 37 {
geovas 0:341347d8ccbc 38 analogWrite(&LM1,0);
geovas 0:341347d8ccbc 39 analogWrite(&LM2,spd);
geovas 0:341347d8ccbc 40 analogWrite(&RM1,0);
geovas 0:341347d8ccbc 41 analogWrite(&RM2,spd);
geovas 0:341347d8ccbc 42 }
geovas 0:341347d8ccbc 43
geovas 0:341347d8ccbc 44 void TurnRight(char spd)
geovas 0:341347d8ccbc 45 {
geovas 0:341347d8ccbc 46 analogWrite(&LM1,spd);
geovas 0:341347d8ccbc 47 analogWrite(&LM2,0);
geovas 0:341347d8ccbc 48 analogWrite(&RM1,0);
geovas 0:341347d8ccbc 49 analogWrite(&RM2,spd);
geovas 0:341347d8ccbc 50 }
geovas 0:341347d8ccbc 51
geovas 0:341347d8ccbc 52 void TurnLeft(char spd)
geovas 0:341347d8ccbc 53 {
geovas 0:341347d8ccbc 54
geovas 0:341347d8ccbc 55 analogWrite(&LM1,0);
geovas 0:341347d8ccbc 56 analogWrite(&LM2,spd);
geovas 0:341347d8ccbc 57 analogWrite(&RM1,spd);
geovas 0:341347d8ccbc 58 analogWrite(&RM2,0);
geovas 0:341347d8ccbc 59 }
geovas 0:341347d8ccbc 60
geovas 0:341347d8ccbc 61 /*int getEcho(void) // for a future...
geovas 0:341347d8ccbc 62 {
geovas 0:341347d8ccbc 63 long duration, distance;
geovas 0:341347d8ccbc 64 digitalWrite(trigPin,LOW);
geovas 0:341347d8ccbc 65 delayMicroseconds(2000);
geovas 0:341347d8ccbc 66 digitalWrite(trigPin, HIGH);
geovas 0:341347d8ccbc 67 delayMicroseconds(1000);
geovas 0:341347d8ccbc 68 digitalWrite(trigPin, LOW);
geovas 0:341347d8ccbc 69
geovas 0:341347d8ccbc 70 duration = pulseIn(echoPin, HIGH);
geovas 0:341347d8ccbc 71 distance = (duration/2) / 29.1;
geovas 0:341347d8ccbc 72
geovas 0:341347d8ccbc 73 if (distance > 200) return 200;
geovas 0:341347d8ccbc 74 if (distance < 0) return 0;
geovas 0:341347d8ccbc 75 return distance;
geovas 0:341347d8ccbc 76 }*/
geovas 0:341347d8ccbc 77
geovas 0:341347d8ccbc 78 int charToInt(char a, char b, char c)
geovas 0:341347d8ccbc 79 {
geovas 0:341347d8ccbc 80 int ans;
geovas 0:341347d8ccbc 81 if (a>'9' || b>'9' || c>'9') return 0;
geovas 0:341347d8ccbc 82 if (a<'0' || b<'0' || c<'0') return 0;
geovas 0:341347d8ccbc 83 ans = (a - 48)*100 + (b - 48)*10 + (c - 48);
geovas 0:341347d8ccbc 84 if (ans > 255) ans = 255;
geovas 0:341347d8ccbc 85 return ans;
geovas 0:341347d8ccbc 86 }
geovas 0:341347d8ccbc 87
geovas 0:341347d8ccbc 88 char str[4];
geovas 0:341347d8ccbc 89 void intToChar(int dat)
geovas 0:341347d8ccbc 90 {
geovas 0:341347d8ccbc 91 int d = dat;
geovas 0:341347d8ccbc 92
geovas 0:341347d8ccbc 93 if (dat<100) str[0] = '0';
geovas 0:341347d8ccbc 94 else
geovas 0:341347d8ccbc 95 {
geovas 0:341347d8ccbc 96 str[0] = d/100 + 48;
geovas 0:341347d8ccbc 97 d %= 100;
geovas 0:341347d8ccbc 98 }
geovas 0:341347d8ccbc 99 if (dat<10) str[1] = '0';
geovas 0:341347d8ccbc 100 else
geovas 0:341347d8ccbc 101 {
geovas 0:341347d8ccbc 102 str[1] = d/10 + 48;
geovas 0:341347d8ccbc 103 d %= 10;
geovas 0:341347d8ccbc 104 }
geovas 0:341347d8ccbc 105 str[2] = d + 48;
geovas 0:341347d8ccbc 106 }
geovas 0:341347d8ccbc 107
geovas 0:341347d8ccbc 108 char toupper(char ch)
geovas 0:341347d8ccbc 109 {
geovas 0:341347d8ccbc 110 if (ch >= 'a' && ch <= 'z') return ch;
geovas 0:341347d8ccbc 111 if (ch >= 'A' && ch <= 'Z') return ch - 'A' + 'a';
geovas 0:341347d8ccbc 112 return 0;
geovas 0:341347d8ccbc 113 }
geovas 0:341347d8ccbc 114
geovas 0:341347d8ccbc 115 int main(void)
geovas 0:341347d8ccbc 116 {
geovas 0:341347d8ccbc 117 char dat[5], b, i;
geovas 0:341347d8ccbc 118 link.baud(9600);
geovas 0:341347d8ccbc 119 //pinMode(trigPin, OUTPUT); // for a future...
geovas 0:341347d8ccbc 120 //pinMode(echoPin, INPUT);
geovas 0:341347d8ccbc 121 GoForward(0); // stop all motors
geovas 0:341347d8ccbc 122
geovas 0:341347d8ccbc 123 while (link.readable()); // trying to fount the starting byte
geovas 0:341347d8ccbc 124 while (link.getc() != 'Z');
geovas 0:341347d8ccbc 125
geovas 0:341347d8ccbc 126 while (true)
geovas 0:341347d8ccbc 127 {
geovas 0:341347d8ccbc 128 if(link.readable()) // read data from blutooth serial port
geovas 0:341347d8ccbc 129 {
geovas 0:341347d8ccbc 130 for (i=0; i<5; i++)
geovas 0:341347d8ccbc 131 dat[i] = link.getc();
geovas 0:341347d8ccbc 132 }
geovas 0:341347d8ccbc 133
geovas 0:341347d8ccbc 134 b = toupper(dat[0]);
geovas 0:341347d8ccbc 135
geovas 0:341347d8ccbc 136 int spd = charToInt(dat[1],dat[2],dat[3]);
geovas 0:341347d8ccbc 137
geovas 0:341347d8ccbc 138 if (b == 'F') GoForward(spd);
geovas 0:341347d8ccbc 139 if (b == 'B') GoBack(spd);
geovas 0:341347d8ccbc 140 if (b == 'R') TurnRight(spd);
geovas 0:341347d8ccbc 141 if (b == 'L') TurnLeft(spd);
geovas 0:341347d8ccbc 142 if (b == 'N') GoForward(0);
geovas 0:341347d8ccbc 143
geovas 0:341347d8ccbc 144 if (b == 'V') // voltmeter
geovas 0:341347d8ccbc 145 {
geovas 0:341347d8ccbc 146 //intToChar(analogRead(volt_Pin) / 4); // for a future...
geovas 0:341347d8ccbc 147 //Bridge.put("DAT",str);
geovas 0:341347d8ccbc 148 }
geovas 0:341347d8ccbc 149 if (b == 'E') // echo sensor
geovas 0:341347d8ccbc 150 {
geovas 0:341347d8ccbc 151 //intToChar(getEcho()); // for a future...
geovas 0:341347d8ccbc 152 //Bridge.put("DAT",str);
geovas 0:341347d8ccbc 153 }
geovas 0:341347d8ccbc 154 }
geovas 0:341347d8ccbc 155 }