Diff: main.cpp

Revision:

0:341347d8ccbc

Child:

1:29eeaabaa8e1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 15 15:09:43 2014 +0000
@@ -0,0 +1,155 @@
+#include "mbed.h"
+
+#define trigPin_  PTC11
+#define echoPin_  PTC12
+#define volt_Pin_ PTB11
+#define LM1_ D3
+#define LM2_ D5
+#define RM1_ D6
+#define RM2_ D7
+#define RX_  D0
+#define TX_  D1
+#define SPEED_ 64
+
+PwmOut LM1(LM1_); // motors configuration
+PwmOut LM2(LM2_);
+PwmOut RM1(RM1_);
+PwmOut RM2(LM2_);
+
+//Serial link(TX_, RX_); // bluetooth serial port
+Serial link(USBTX, USBRX); // usb serial port
+ 
+void analogWrite(mbed::PwmOut *prt, char spd) // write to PWM
+{
+  float a = spd / 2.56; // convert 0..256 to '%'
+  *prt = a;
+}
+
+void GoForward(char spd)
+{
+  analogWrite(&LM1,spd);
+  analogWrite(&LM2,0);
+  analogWrite(&RM1,spd);
+  analogWrite(&RM2,0);
+}
+
+void GoBack(char spd)
+{
+  analogWrite(&LM1,0);
+  analogWrite(&LM2,spd);
+  analogWrite(&RM1,0);
+  analogWrite(&RM2,spd);
+}
+
+void TurnRight(char spd)
+{
+  analogWrite(&LM1,spd);
+  analogWrite(&LM2,0);
+  analogWrite(&RM1,0);
+  analogWrite(&RM2,spd);
+}
+
+void TurnLeft(char spd)
+{
+
+  analogWrite(&LM1,0);
+  analogWrite(&LM2,spd);
+  analogWrite(&RM1,spd);
+  analogWrite(&RM2,0);
+}
+
+/*int getEcho(void) // for a future...
+{
+  long duration, distance;
+  digitalWrite(trigPin,LOW);
+  delayMicroseconds(2000);
+  digitalWrite(trigPin, HIGH);
+  delayMicroseconds(1000);
+  digitalWrite(trigPin, LOW);
+  
+  duration = pulseIn(echoPin, HIGH);
+  distance = (duration/2) / 29.1;
+  
+  if (distance > 200) return 200;
+  if (distance < 0) return 0;
+  return distance;
+}*/
+
+int charToInt(char a, char b, char c)
+{
+  int ans;
+  if (a>'9' || b>'9' || c>'9') return 0;
+  if (a<'0' || b<'0' || c<'0') return 0;
+  ans = (a - 48)*100 + (b - 48)*10 + (c - 48);
+  if (ans > 255) ans = 255;
+  return ans;
+}
+
+char str[4];
+void intToChar(int dat)
+{
+  int d = dat;
+  
+  if (dat<100) str[0] = '0';
+  else
+  {
+    str[0] = d/100 + 48;
+    d %= 100;
+  }
+  if (dat<10) str[1] = '0';
+  else
+  {
+    str[1] = d/10 + 48;
+    d %= 10;
+  }
+  str[2] = d + 48;
+}
+
+char toupper(char ch)
+{
+  if (ch >= 'a' && ch <= 'z') return ch;
+  if (ch >= 'A' && ch <= 'Z') return ch - 'A' + 'a';
+  return 0;
+}
+
+int main(void)
+{
+   char dat[5], b, i;
+   link.baud(9600);
+   //pinMode(trigPin, OUTPUT); // for a future...
+   //pinMode(echoPin, INPUT);
+   GoForward(0); // stop all motors
+
+   while (link.readable());    // trying to fount the starting byte
+   while (link.getc() != 'Z');
+
+   while (true)
+   {
+     if(link.readable()) // read data from blutooth serial port
+     {
+        for (i=0; i<5; i++)
+        dat[i] = link.getc();
+     }
+     
+     b = toupper(dat[0]);
+   
+     int spd = charToInt(dat[1],dat[2],dat[3]);
+   
+     if (b == 'F') GoForward(spd);
+     if (b == 'B') GoBack(spd);
+     if (b == 'R') TurnRight(spd);
+     if (b == 'L') TurnLeft(spd);
+     if (b == 'N') GoForward(0);
+   
+     if (b == 'V') // voltmeter
+     {
+       //intToChar(analogRead(volt_Pin) / 4); // for a future...
+       //Bridge.put("DAT",str);
+     }
+     if (b == 'E') // echo sensor
+     {
+       //intToChar(getEcho()); // for a future...
+       //Bridge.put("DAT",str);
+     }
+   }
+}
\ No newline at end of file