Team Virgo v3 / Orion_newPCB_test_LV

Implement new controller

Dependencies:   mbed-rtos mbed QEI BNO055 MPU6050_DMP_Nucleo-I2Cdev virgo3_imuHandler_Orion_PCB MAX17048 Servo

Fork of Orion_newPCB_test by Team Virgo v3

Diff: orion_main.cpp

Revision:
25:3a82f868c101
Parent:
23:6806c3bacf58
Child:
26:32eaf3c3ac2e
--- a/orion_main.cpp	Tue Mar 13 10:10:02 2018 +0000
+++ b/orion_main.cpp	Tue Mar 13 12:32:04 2018 +0000
@@ -340,10 +340,10 @@
 
 
             if(waypointSetFinish_flag == 0) {
-//                rpm_cmd[0]=W_l*60/(2*M_PI)*(-1.0);
-//                rpm_cmd[1]=W_r*60/(2*M_PI)*(-1.0);
-                rpm_cmd[0]=-800;
-                rpm_cmd[1]=-800;
+                rpm_cmd[0]=W_l*60/(2*M_PI)*(-1.0);
+                rpm_cmd[1]=W_r*60/(2*M_PI)*(-1.0);
+//                rpm_cmd[0]=-800;
+//                rpm_cmd[1]=-800;
 
                 if( (generalFunctions::abs_f(rpm_cmd[0]) < 500.0) && (generalFunctions::abs_f(rpm_cmd[0]) > 100.0) )
                     rpm_cmd[0] = 475.0*generalFunctions::sign_f(rpm_cmd[0]);