Added Back ToF code
Dependencies: QEI2 PID IMU6050Ver11 Watchdog VL53L1X_Filter ros_lib_kinetic
Dependents: Version1-6 Version1-7
Diff: wheelchair.cpp
- Revision:
- 4:29a27953fe70
- Parent:
- 3:a5e71bfdb492
- Child:
- 5:e0ccaab3959a
diff -r a5e71bfdb492 -r 29a27953fe70 wheelchair.cpp --- a/wheelchair.cpp Mon Jul 16 20:46:42 2018 +0000 +++ b/wheelchair.cpp Tue Jul 17 02:29:26 2018 +0000 @@ -10,9 +10,12 @@ */ void Wheelchair::move(float x_coor, float y_coor) { - printf("x is %f y is %f \n", 1.6*x_coor + 2.5, 1.6*y_coor + 2.5 + offset); - x->write(1.6*x_coor + 2.5); - y->write(1.6*y_coor + 2.5 + offset); + printf("raw is %f %f \n", x_coor, y_coor); + printf("x is %f y is %f \n", ((x_coor*1.6f) + 1.7f), ((y_coor*1.6f) + 1.7f)); + float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f; + float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f; + x->write(scaled_x); + y->write(scaled_y); }