File content as of revision 1:3125b4958359:

 
#include "mbed.h"
#include    "INA219.h"
//int p_sda = PB_7;
//int p_scl = PB_6;
//INA219::INA219 (p_sda, p_scl, const INA219_TypeDef *ina219_parameter)
 //: _i2c_p(new I2C(p_sda, p_scl)), _i2c(*_i2c_p)
 
using namespace std::chrono;
 
//#define ROBO_DEBUG
 
 
#define MAX_NR_OF_SERVOS 9
#define MSG_TERMINATOR '\n'
 
 
PwmOut S_1(PA_12); PwmOut S_2(PB_0); PwmOut S_3(PB_4); PwmOut S_4(PB_5); PwmOut S_5(PA_8); PwmOut S_6(PA_11);
// 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=8ms;
  servoVec[actualNrOfServos++] = new cCustomServo(D10, 0, 180, 90, speed); // servo baza A
  //servoVec[actualNrOfServos++] = new cCustomServo(PB_0, 0, 180, 90, speed); // servo cot C 
  servoVec[actualNrOfServos++] = new cCustomServo(D2, 0, 180, 90, speed); // servo rotatie cleste D 
  servoVec[actualNrOfServos++] = new cCustomServo(D9, 0, 180, 90, speed); // nu raspunde
  servoVec[actualNrOfServos++] = new cCustomServo(D11, 0, 180, 90, speed); //  Servo cot nu raspunde
  servoVec[actualNrOfServos++] = new cCustomServo(D12, 0, 180, 90, speed); // servo nu raspunde
//PwmOut S_1(PA_12); PwmOut S_2(PB_0); PwmOut S_3(PB_4); PwmOut S_4(PB_5); PwmOut S_5(PA_8); PwmOut S_6(PA_11); 
  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]);
      }
    }
  }
}