Adrian Balahura / Mbed 2 deprecated BratRobot

robot arm with servos

Dependencies:   mbed INA219

main.cpp@0:4d49538f919b, 2020-09-09 (annotated)

Committer:
vsandule
Date:
Wed Sep 09 06:16:50 2020 +0000
Revision:
0:4d49538f919b
Child:
1:3125b4958359
First rev

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsandule 0:4d49538f919b 1
vsandule 0:4d49538f919b 2 #include "mbed.h"
vsandule 0:4d49538f919b 3
vsandule 0:4d49538f919b 4 using namespace std::chrono;
vsandule 0:4d49538f919b 5
vsandule 0:4d49538f919b 6
vsandule 0:4d49538f919b 7 //#define ROBO_DEBUG
vsandule 0:4d49538f919b 8
vsandule 0:4d49538f919b 9
vsandule 0:4d49538f919b 10 #define MAX_NR_OF_SERVOS 9
vsandule 0:4d49538f919b 11 #define MSG_TERMINATOR '\n'
vsandule 0:4d49538f919b 12
vsandule 0:4d49538f919b 13 // Create a BufferedSerial object to be used by the system I/O retarget code.
vsandule 0:4d49538f919b 14 static BufferedSerial serial_port(PA_2, PA_15, 115200);
vsandule 0:4d49538f919b 15
vsandule 0:4d49538f919b 16 FileHandle *mbed::mbed_override_console(int fd) { return &serial_port; }
vsandule 0:4d49538f919b 17
vsandule 0:4d49538f919b 18 static BufferedSerial bluetooth(PA_9, PA_10, 9600);
vsandule 0:4d49538f919b 19
vsandule 0:4d49538f919b 20 inline float DegToProc(float deg) {
vsandule 0:4d49538f919b 21 // 180 g -> 2ms -> 10% of 20ms -> 0.1f
vsandule 0:4d49538f919b 22 // 0 g -> 1ms -> 5% of 20ms -> 0.05f
vsandule 0:4d49538f919b 23 // x = y; y=x*0.1/180
vsandule 0:4d49538f919b 24 return deg/180*1500 + 800;
vsandule 0:4d49538f919b 25 }
vsandule 0:4d49538f919b 26
vsandule 0:4d49538f919b 27 class cCustomServo {
vsandule 0:4d49538f919b 28
vsandule 0:4d49538f919b 29 public:
vsandule 0:4d49538f919b 30 PwmOut *_sPtr;
vsandule 0:4d49538f919b 31
vsandule 0:4d49538f919b 32 PinName _pin;
vsandule 0:4d49538f919b 33 int _min;
vsandule 0:4d49538f919b 34 int _max;
vsandule 0:4d49538f919b 35 int _resetAngle;
vsandule 0:4d49538f919b 36 Kernel::Clock::duration_u32 _speed;
vsandule 0:4d49538f919b 37 int _curPos;
vsandule 0:4d49538f919b 38
vsandule 0:4d49538f919b 39 public:
vsandule 0:4d49538f919b 40 cCustomServo(PinName pin, int minAngle, int maxAngle, int resetAngle,
vsandule 0:4d49538f919b 41 Kernel::Clock::duration_u32 speed) {
vsandule 0:4d49538f919b 42 _pin = pin;
vsandule 0:4d49538f919b 43 _min = minAngle;
vsandule 0:4d49538f919b 44 _max = maxAngle;
vsandule 0:4d49538f919b 45 _resetAngle = resetAngle;
vsandule 0:4d49538f919b 46 _speed = speed;
vsandule 0:4d49538f919b 47
vsandule 0:4d49538f919b 48 if (_resetAngle < _min)
vsandule 0:4d49538f919b 49 _resetAngle = _min;
vsandule 0:4d49538f919b 50 if (_resetAngle > _max)
vsandule 0:4d49538f919b 51 _resetAngle = _max;
vsandule 0:4d49538f919b 52
vsandule 0:4d49538f919b 53 _sPtr = new PwmOut(pin);
vsandule 0:4d49538f919b 54 _sPtr->period_ms(20);
vsandule 0:4d49538f919b 55
vsandule 0:4d49538f919b 56 _curPos = _resetAngle;
vsandule 0:4d49538f919b 57 }
vsandule 0:4d49538f919b 58
vsandule 0:4d49538f919b 59 int getCurrentAngle() { return _curPos; }
vsandule 0:4d49538f919b 60
vsandule 0:4d49538f919b 61 void movTo(int newPos) {
vsandule 0:4d49538f919b 62
vsandule 0:4d49538f919b 63 if (newPos <= _min)
vsandule 0:4d49538f919b 64 newPos = _min;
vsandule 0:4d49538f919b 65 if (newPos >= _max)
vsandule 0:4d49538f919b 66 newPos = _max;
vsandule 0:4d49538f919b 67
vsandule 0:4d49538f919b 68 if (newPos >= _curPos) {
vsandule 0:4d49538f919b 69 for (int i = _curPos; i <= newPos; i++) {
vsandule 0:4d49538f919b 70 float x = DegToProc(i);
vsandule 0:4d49538f919b 71 _sPtr->pulsewidth_us(x);
vsandule 0:4d49538f919b 72 // printf("U: %3d X:%f\n",i,x);
vsandule 0:4d49538f919b 73 ThisThread::sleep_for(_speed);
vsandule 0:4d49538f919b 74 }
vsandule 0:4d49538f919b 75 } else if (newPos <= _curPos) {
vsandule 0:4d49538f919b 76 for (int i = _curPos; i >= newPos; i--) {
vsandule 0:4d49538f919b 77 float x = DegToProc(i);
vsandule 0:4d49538f919b 78 _sPtr->pulsewidth_us(x);
vsandule 0:4d49538f919b 79 // printf("U: %3d X: %f \n",i,x);
vsandule 0:4d49538f919b 80 ThisThread::sleep_for(_speed);
vsandule 0:4d49538f919b 81 }
vsandule 0:4d49538f919b 82 }
vsandule 0:4d49538f919b 83
vsandule 0:4d49538f919b 84 _curPos = newPos;
vsandule 0:4d49538f919b 85 }
vsandule 0:4d49538f919b 86
vsandule 0:4d49538f919b 87 void reset() { movTo(_resetAngle); }
vsandule 0:4d49538f919b 88 };
vsandule 0:4d49538f919b 89
vsandule 0:4d49538f919b 90 //---------------------------------------------------------------
vsandule 0:4d49538f919b 91
vsandule 0:4d49538f919b 92 inline bool isDigit(char a) { return (a >= '0') && (a <= '9'); }
vsandule 0:4d49538f919b 93
vsandule 0:4d49538f919b 94 int main() {
vsandule 0:4d49538f919b 95 printf("--Start Prog--\n");
vsandule 0:4d49538f919b 96
vsandule 0:4d49538f919b 97
vsandule 0:4d49538f919b 98 Timer t;
vsandule 0:4d49538f919b 99 // Servo instantiation
vsandule 0:4d49538f919b 100 cCustomServo *servoVec[MAX_NR_OF_SERVOS];
vsandule 0:4d49538f919b 101 // vector of new angles we receive from the app
vsandule 0:4d49538f919b 102 int newAngles[MAX_NR_OF_SERVOS];
vsandule 0:4d49538f919b 103
vsandule 0:4d49538f919b 104 int actualNrOfServos = 0;
vsandule 0:4d49538f919b 105
vsandule 0:4d49538f919b 106
vsandule 0:4d49538f919b 107 // pin | min | max | reset | speed
vsandule 0:4d49538f919b 108 // connected 5 servos (5-9)
vsandule 0:4d49538f919b 109 Kernel::Clock::duration_u32 speed=10ms;
vsandule 0:4d49538f919b 110 servoVec[actualNrOfServos++] = new cCustomServo(A2, 0, 180, 90, speed); // umar
vsandule 0:4d49538f919b 111 servoVec[actualNrOfServos++] = new cCustomServo(D3, 0, 180, 90, speed); // cot1
vsandule 0:4d49538f919b 112 servoVec[actualNrOfServos++] = new cCustomServo(D4, 0, 180, 90, speed); // cot2
vsandule 0:4d49538f919b 113 // servoVec[actualNrOfServos++] = new cCustomServo(D7, 15, 165, 90, speed); // cot3 !!!deconectat!!!
vsandule 0:4d49538f919b 114 servoVec[actualNrOfServos++] = new cCustomServo(D5, 0, 180, 90, speed); // rotatie cleste
vsandule 0:4d49538f919b 115 servoVec[actualNrOfServos++] = new cCustomServo(D6, 0, 65, 21, speed); // deschidere cleste
vsandule 0:4d49538f919b 116
vsandule 0:4d49538f919b 117 for (int i = 0; i < actualNrOfServos; i++) {
vsandule 0:4d49538f919b 118 servoVec[i]->reset();
vsandule 0:4d49538f919b 119 newAngles[i] = servoVec[i]->getCurrentAngle();
vsandule 0:4d49538f919b 120 }
vsandule 0:4d49538f919b 121
vsandule 0:4d49538f919b 122 unsigned long PrevMovTime = 0;
vsandule 0:4d49538f919b 123 unsigned long CurrentTime = 0;
vsandule 0:4d49538f919b 124
vsandule 0:4d49538f919b 125 t.start();
vsandule 0:4d49538f919b 126
vsandule 0:4d49538f919b 127 char dataIn[50];
vsandule 0:4d49538f919b 128 int dataIdx=0;
vsandule 0:4d49538f919b 129
vsandule 0:4d49538f919b 130
vsandule 0:4d49538f919b 131 while (true) {
vsandule 0:4d49538f919b 132 char c;
vsandule 0:4d49538f919b 133
vsandule 0:4d49538f919b 134
vsandule 0:4d49538f919b 135 if (bluetooth.readable()) {
vsandule 0:4d49538f919b 136 bluetooth.read(&c, 1);
vsandule 0:4d49538f919b 137 dataIn[dataIdx++]= c;
vsandule 0:4d49538f919b 138
vsandule 0:4d49538f919b 139
vsandule 0:4d49538f919b 140 if (c == MSG_TERMINATOR) {
vsandule 0:4d49538f919b 141 //#ifdef ROBO_DEBUG
vsandule 0:4d49538f919b 142 // printf("Rec:%s\n",dataIn);
vsandule 0:4d49538f919b 143 //#endif
vsandule 0:4d49538f919b 144 if (dataIn[0]=='s') //<s><servo_Sel><angle_part><angle_part><angle_part>
vsandule 0:4d49538f919b 145 {
vsandule 0:4d49538f919b 146 bool msgOk = true;
vsandule 0:4d49538f919b 147 int servoIdx;
vsandule 0:4d49538f919b 148 int desiredAngle = 0;
vsandule 0:4d49538f919b 149
vsandule 0:4d49538f919b 150 msgOk = msgOk && isDigit(dataIn[1]) && dataIdx >= 3; // s + sevo_num + (angle<10)
vsandule 0:4d49538f919b 151
vsandule 0:4d49538f919b 152 servoIdx = dataIn[1] - '0';
vsandule 0:4d49538f919b 153
vsandule 0:4d49538f919b 154 msgOk = msgOk && (servoIdx >= 0) && (servoIdx < actualNrOfServos);
vsandule 0:4d49538f919b 155
vsandule 0:4d49538f919b 156
vsandule 0:4d49538f919b 157 // parse angle digits
vsandule 0:4d49538f919b 158 for (int i = 2; i < dataIdx -1 ;i++) { //-1 due to MSG_TERMINATOR
vsandule 0:4d49538f919b 159 msgOk = msgOk && isDigit(dataIn[i]);
vsandule 0:4d49538f919b 160 if (msgOk) {
vsandule 0:4d49538f919b 161 desiredAngle = desiredAngle * 10 + (dataIn[i] - '0');
vsandule 0:4d49538f919b 162 }
vsandule 0:4d49538f919b 163 }
vsandule 0:4d49538f919b 164
vsandule 0:4d49538f919b 165 if (msgOk) {
vsandule 0:4d49538f919b 166 newAngles[servoIdx] = desiredAngle;
vsandule 0:4d49538f919b 167 #ifdef ROBO_DEBUG
vsandule 0:4d49538f919b 168 printf("Angle[%d]=%d\n",servoIdx,desiredAngle);
vsandule 0:4d49538f919b 169 #endif
vsandule 0:4d49538f919b 170 }
vsandule 0:4d49538f919b 171 }
vsandule 0:4d49538f919b 172
vsandule 0:4d49538f919b 173 // we get here when we pare a full string containing a terminator
vsandule 0:4d49538f919b 174 // dataIn needs to be reset for further concatenations (+=)
vsandule 0:4d49538f919b 175 dataIdx = 0;
vsandule 0:4d49538f919b 176 }
vsandule 0:4d49538f919b 177 }
vsandule 0:4d49538f919b 178
vsandule 0:4d49538f919b 179 // Restrict the ammount of changes we do
vsandule 0:4d49538f919b 180 CurrentTime = duration_cast<milliseconds>(t.elapsed_time()).count();
vsandule 0:4d49538f919b 181 if (CurrentTime - PrevMovTime > 100) {
vsandule 0:4d49538f919b 182 PrevMovTime = CurrentTime;
vsandule 0:4d49538f919b 183
vsandule 0:4d49538f919b 184 #ifdef ROBO_DEBUG
vsandule 0:4d49538f919b 185 printf("Updated Angles: %3d %3d %3d %3d %3d\n",newAngles[0],newAngles[1],newAngles[2],newAngles[3],newAngles[4]);
vsandule 0:4d49538f919b 186 #endif
vsandule 0:4d49538f919b 187
vsandule 0:4d49538f919b 188 for (int i = 0; i < actualNrOfServos; i++) {
vsandule 0:4d49538f919b 189 servoVec[i]->movTo(newAngles[i]);
vsandule 0:4d49538f919b 190 }
vsandule 0:4d49538f919b 191 }
vsandule 0:4d49538f919b 192 }
vsandule 0:4d49538f919b 193 }
vsandule 0:4d49538f919b 194
vsandule 0:4d49538f919b 195