Diff: SDC21XX_Motor.cpp

Revision:

1:a677037eca0a

Parent:

0:1c0bc44980e0

Child:

2:30acd3c5d5da

diff -r 1c0bc44980e0 -r a677037eca0a SDC21XX_Motor.cpp
--- a/SDC21XX_Motor.cpp	Wed Jun 01 15:03:34 2016 +0000
+++ b/SDC21XX_Motor.cpp	Mon Jul 11 11:02:54 2016 +0000
@@ -3,18 +3,24 @@
 
 namespace mbed {
     
-    SDC21XX_Motor::SDC21XX_Motor(uint8_t _node_id, CAN * _can, uint8_t _channel, uint32_t _reductor): channel(_channel), reductor(_reductor), RB(_node_id, _can)
+    SDC21XX_Motor::SDC21XX_Motor(uint8_t _node_id, CAN * _can, uint8_t _channel, uint32_t _reductor, uint32_t _enc_resolution): RB(_node_id, _can), channel(_channel), reductor(_reductor), enc_resolution(_enc_resolution)
     {
         
     }
+    
+    SDC21XX_Motor::SDC21XX_Motor(Roboteq *_RB, uint8_t _channel, uint32_t _reductor, uint32_t _enc_resolution): RB(_RB->GetNodeID(), _RB->GetCan()), channel(_channel), reductor(_reductor), enc_resolution(_enc_resolution)
+    {
+        
+    }
+    
     void SDC21XX_Motor::GoAngle(int16_t _angle)
     {
-        RB.SetPosition(_angle*1024/360*reductor, channel);
+        RB.SetPosition(_angle*enc_resolution*reductor/360, channel);
     }
     
     int16_t SDC21XX_Motor::GetAngle()
     {
-        return RB.ReadAbsoluteEncoderCount(channel);
+        return RB.ReadAbsoluteEncoderCount(channel)*360/enc_resolution/reductor;
     }
     
     void SDC21XX_Motor::SetChannel(uint8_t _channel)