Diff: AX12.cpp

Revision:

1:19cd7097b3e4

Parent:

0:6062259b2143

Child:

2:4b23ea069b7e

diff -r 6062259b2143 -r 19cd7097b3e4 AX12.cpp
--- a/AX12.cpp	Sun Mar 25 20:38:22 2018 +0000
+++ b/AX12.cpp	Mon Mar 26 10:42:09 2018 +0000
@@ -24,13 +24,11 @@
 #include "AX12.h"
 #include "mbed.h"
 
-AX12::AX12(PinName tx, PinName rx, int ID, int startAngle)
+AX12::AX12(PinName tx, PinName rx, int ID)
      : _ax12(tx,rx) {
    int  _baud = 1000000;
     _ID = ID;
     _ax12.baud(_baud);
-    _timedMove_isInProgress = 0;
-    _timedMove_angleCurrent = startAngle;
 }
 
 // Set the mode of the servo
@@ -262,46 +260,14 @@
 }
 
 
-void AX12::SetTimedMove(int goal, int micros, int currentMicros) {
-    _timedMove_isInProgress = 1;
-    _timedMove_angleGoal = goal;
-    if (goal == _timedMove_angleCurrent) {
-        _timedMove_isInProgress = 0;
-    } else {
-        int diff = goal-_timedMove_angleCurrent;
-        if (diff > 0) {
-            _timedMove_periodMicro = micros/diff;
-            _timedMove_step = 1;
-        } else {
-            _timedMove_periodMicro = micros/(-diff);
-            _timedMove_step = -1;
-        }
-        _timedMove_lastMicro = currentMicros;
-    }
-}
-
-
-void AX12::updateTimedMove(int currentMicros) {
-    if (currentMicros >= _timedMove_lastMicro+_timedMove_periodMicro) {
-        int numberStep = (currentMicros-_timedMove_lastMicro)/_timedMove_periodMicro;
-        _timedMove_angleCurrent = _timedMove_angleCurrent+(_timedMove_step*numberStep);
-        SetGoal(_timedMove_angleCurrent);
-        if (_timedMove_angleCurrent == _timedMove_angleGoal) {
-            _timedMove_isInProgress = 0;
-        } else {
-            _timedMove_lastMicro = currentMicros;
-        }
-    }
-}
-
-
-int AX12::isTimedMoveInProgress() {
-    return _timedMove_isInProgress;
-}
-
-
-int AX12::getCurrentAngle() {
-    return _timedMove_angleCurrent;
+void AX12::setSpeedMove(int goal, int speed) {
+    char data[2];
+    data[0] = speed & 0xff;
+    data[1] = speed >> 8;
+    
+    write(_ID, 0x20, 2, data);
+    
+    SetGoal(goal);
 }