FIX: position '0' is when opened and '255' when closed (calculation)

slimasziom · slimasziom · commit 7e16a62b3569 · 2025-02-04T10:32:25.000+01:00
diff --git a/robotiq_hande_driver/src/application.cpp b/robotiq_hande_driver/src/application.cpp
@@ -67,7 +67,7 @@ double ApplicationLayer::Position(){
 }
 
 void ApplicationLayer::SetPosition(double position){
-    uint8_t raw_position = uint8_t(position / kGripperPositionStep - kGripperPositionMin);
+    uint8_t raw_position = uint8_t(kGripperPositionMax - position / kGripperPositionStep);
 
     protocol_logic_.GoTo(raw_position, 255, 255);
 }
@@ -90,7 +90,7 @@ void ApplicationLayer::Read(){
     //fault_status
 
     requested_position_ = (double)protocol_logic_.GetRegPos() * kGripperPositionStep + kGripperPositionMin;
-    position_ = (double)protocol_logic_.GetPos() * kGripperPositionStep + kGripperPositionMin;
+    position_ = kGripperPositionMax - (double)protocol_logic_.GetPos() * kGripperPositionStep;
     current_ = (double)protocol_logic_.GetCurrent() * kGripperCurrentScale;
 }
 

Original file line number	Diff line number	Diff line change
`@@ -67,7 +67,7 @@ double ApplicationLayer::Position(){`
`67`	`67`	`}`
`68`	`68`
`69`	`69`	`void ApplicationLayer::SetPosition(double position){`
`70`		`- uint8_t raw_position = uint8_t(position / kGripperPositionStep - kGripperPositionMin);`
	`70`	`+ uint8_t raw_position = uint8_t(kGripperPositionMax - position / kGripperPositionStep);`
`71`	`71`
`72`	`72`	`protocol_logic_.GoTo(raw_position, 255, 255);`
`73`	`73`	`}`
`@@ -90,7 +90,7 @@ void ApplicationLayer::Read(){`
`90`	`90`	`//fault_status`
`91`	`91`
`92`	`92`	`requested_position_ = (double)protocol_logic_.GetRegPos() * kGripperPositionStep + kGripperPositionMin;`
`93`		`- position_ = (double)protocol_logic_.GetPos() * kGripperPositionStep + kGripperPositionMin;`
	`93`	`+ position_ = kGripperPositionMax - (double)protocol_logic_.GetPos() * kGripperPositionStep;`
`94`	`94`	`current_ = (double)protocol_logic_.GetCurrent() * kGripperCurrentScale;`
`95`	`95`	`}`
`96`	`96`