Diff: main.cpp

Revision:

0:4d49538f919b

Child:

1:3125b4958359

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Sep 09 06:16:50 2020 +0000
@@ -0,0 +1,195 @@
+ 
+#include "mbed.h"
+ 
+using namespace std::chrono;
+ 
+ 
+//#define ROBO_DEBUG
+ 
+ 
+#define MAX_NR_OF_SERVOS 9
+#define MSG_TERMINATOR '\n'
+ 
+// Create a BufferedSerial object to be used by the system I/O retarget code.
+static BufferedSerial serial_port(PA_2, PA_15, 115200);
+ 
+FileHandle *mbed::mbed_override_console(int fd) { return &serial_port; }
+ 
+static BufferedSerial bluetooth(PA_9, PA_10, 9600);
+ 
+inline float DegToProc(float deg) { 
+        // 180 g -> 2ms -> 10% of 20ms -> 0.1f
+        // 0   g -> 1ms -> 5% of  20ms -> 0.05f
+        // x = y; y=x*0.1/180
+        return deg/180*1500 + 800; 
+        }
+ 
+class cCustomServo {
+ 
+public:
+  PwmOut *_sPtr;
+ 
+  PinName _pin;
+  int _min;
+  int _max;
+  int _resetAngle;
+  Kernel::Clock::duration_u32 _speed;
+  int _curPos;
+ 
+public:
+  cCustomServo(PinName pin, int minAngle, int maxAngle, int resetAngle,
+               Kernel::Clock::duration_u32 speed) {
+    _pin = pin;
+    _min = minAngle;
+    _max = maxAngle;
+    _resetAngle = resetAngle;
+    _speed = speed;
+ 
+    if (_resetAngle < _min)
+      _resetAngle = _min;
+    if (_resetAngle > _max)
+      _resetAngle = _max;
+ 
+    _sPtr = new PwmOut(pin);
+    _sPtr->period_ms(20);
+ 
+    _curPos = _resetAngle;
+  }
+ 
+  int getCurrentAngle() { return _curPos; }
+ 
+  void movTo(int newPos) {
+ 
+    if (newPos <= _min)
+      newPos = _min;
+    if (newPos >= _max)
+      newPos = _max;
+ 
+    if (newPos >= _curPos) {
+      for (int i = _curPos; i <= newPos; i++) {
+        float x = DegToProc(i);
+        _sPtr->pulsewidth_us(x);
+        // printf("U: %3d X:%f\n",i,x);
+        ThisThread::sleep_for(_speed);
+      }
+    } else if (newPos <= _curPos) {
+      for (int i = _curPos; i >= newPos; i--) {
+        float x = DegToProc(i);
+        _sPtr->pulsewidth_us(x);
+        // printf("U: %3d X: %f \n",i,x);
+        ThisThread::sleep_for(_speed);
+      }
+    }
+ 
+    _curPos = newPos;
+  }
+ 
+  void reset() { movTo(_resetAngle); }
+};
+ 
+//---------------------------------------------------------------
+ 
+inline bool isDigit(char a) { return (a >= '0') && (a <= '9'); }
+ 
+int main() {
+  printf("--Start Prog--\n");
+ 
+ 
+  Timer t;
+  // Servo instantiation
+  cCustomServo *servoVec[MAX_NR_OF_SERVOS];
+  // vector of new angles we receive from the app
+  int newAngles[MAX_NR_OF_SERVOS];
+ 
+  int actualNrOfServos = 0;
+ 
+ 
+  // pin | min | max | reset | speed
+  // connected 5 servos (5-9)
+  Kernel::Clock::duration_u32 speed=10ms;
+  servoVec[actualNrOfServos++] = new cCustomServo(A2, 0, 180, 90, speed); // umar
+  servoVec[actualNrOfServos++] = new cCustomServo(D3, 0, 180, 90, speed); // cot1
+  servoVec[actualNrOfServos++] = new cCustomServo(D4, 0, 180, 90, speed); // cot2
+  // servoVec[actualNrOfServos++] = new cCustomServo(D7, 15, 165, 90, speed); // cot3 !!!deconectat!!!
+  servoVec[actualNrOfServos++] = new cCustomServo(D5, 0, 180, 90, speed); // rotatie cleste
+  servoVec[actualNrOfServos++] = new cCustomServo(D6, 0, 65, 21, speed); // deschidere cleste
+ 
+  for (int i = 0; i < actualNrOfServos; i++) {
+    servoVec[i]->reset();
+    newAngles[i] = servoVec[i]->getCurrentAngle();
+  }
+ 
+  unsigned long PrevMovTime = 0;
+  unsigned long CurrentTime = 0;
+ 
+  t.start();
+ 
+  char dataIn[50];
+  int dataIdx=0;
+ 
+ 
+  while (true) {
+    char c;
+ 
+ 
+    if (bluetooth.readable()) {
+      bluetooth.read(&c, 1);
+      dataIn[dataIdx++]= c;
+ 
+ 
+      if (c == MSG_TERMINATOR) {
+        //#ifdef ROBO_DEBUG
+        //    printf("Rec:%s\n",dataIn);
+        //#endif
+        if (dataIn[0]=='s') //<s><servo_Sel><angle_part><angle_part><angle_part>
+        {
+          bool msgOk = true;
+          int servoIdx;
+          int desiredAngle = 0;
+ 
+          msgOk = msgOk && isDigit(dataIn[1]) && dataIdx >= 3; // s + sevo_num + (angle<10)
+ 
+          servoIdx = dataIn[1] - '0';
+ 
+          msgOk = msgOk && (servoIdx >= 0) && (servoIdx < actualNrOfServos);
+ 
+ 
+          // parse angle digits
+          for (int i = 2; i < dataIdx -1 ;i++) { //-1 due to MSG_TERMINATOR
+            msgOk = msgOk && isDigit(dataIn[i]);
+            if (msgOk) {
+              desiredAngle = desiredAngle * 10 + (dataIn[i] - '0');
+            }
+          }
+ 
+          if (msgOk) {
+            newAngles[servoIdx] = desiredAngle;
+            #ifdef ROBO_DEBUG
+                printf("Angle[%d]=%d\n",servoIdx,desiredAngle);
+            #endif
+          }
+        }
+ 
+        // we get here when we pare a full string containing a terminator
+        // dataIn needs to be reset for further concatenations (+=)
+        dataIdx = 0;
+      }
+    }
+ 
+    // Restrict the ammount of changes we do
+    CurrentTime = duration_cast<milliseconds>(t.elapsed_time()).count();
+    if (CurrentTime - PrevMovTime > 100) {
+      PrevMovTime = CurrentTime;
+    
+    #ifdef ROBO_DEBUG
+        printf("Updated Angles: %3d %3d %3d %3d %3d\n",newAngles[0],newAngles[1],newAngles[2],newAngles[3],newAngles[4]);
+    #endif
+ 
+      for (int i = 0; i < actualNrOfServos; i++) {
+        servoVec[i]->movTo(newAngles[i]);
+      }
+    }
+  }
+}
+ 
+