lab3

Dependencies:   mbed ros_lib_kinetic hallsensor_software_decoder

Revision:
8:82efd1be89ee
Parent:
6:2c7b0e609713
--- a/main.cpp	Fri Mar 29 05:59:59 2019 +0000
+++ b/main.cpp	Sun Mar 31 12:28:42 2019 +0000
@@ -115,8 +115,8 @@
 
 void controllerTISR() {
     // 60.0 / 2pi = 9.549296586
-    controller1.refRPM = (car.refV/car.r - car.L*car.refW/2.0/car.r) / 9.549296586;
-    controller2.refRPM = (car.refV/car.r + car.L*car.refW/2.0/car.r) / 9.549296586;
+    controller1.refRPM = (car.refV - car.L*car.refW/2.0) * 9.549296586 / car.r;
+    controller2.refRPM = (car.refV + car.L*car.refW/2.0) * 9.549296586 / car.r;
     
 //    pc.printf("%.2f, %.2f", controller1.refRPM, controller2.refRPM);