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
05_TrajectoryTracking/kinematics.cpp@10:ea4b90be68dc, 2016-04-23 (annotated)
- Committer:
- akashvibhute
- Date:
- Sat Apr 23 10:29:24 2016 +0000
- Revision:
- 10:ea4b90be68dc
virgo 3.3 with 6v motors
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
akashvibhute | 10:ea4b90be68dc | 1 | #include "kinematics.h" |
akashvibhute | 10:ea4b90be68dc | 2 | |
akashvibhute | 10:ea4b90be68dc | 3 | kinematics::kinematics() |
akashvibhute | 10:ea4b90be68dc | 4 | { |
akashvibhute | 10:ea4b90be68dc | 5 | wheelDia = wheel_dia; |
akashvibhute | 10:ea4b90be68dc | 6 | trackWidth = track_width; |
akashvibhute | 10:ea4b90be68dc | 7 | |
akashvibhute | 10:ea4b90be68dc | 8 | } |
akashvibhute | 10:ea4b90be68dc | 9 | |
akashvibhute | 10:ea4b90be68dc | 10 | void kinematics::VW_RPM(float velocity, float omega, float* left_rpm, float* right_rpm) |
akashvibhute | 10:ea4b90be68dc | 11 | { |
akashvibhute | 10:ea4b90be68dc | 12 | *right_rpm = (velocity + (omega * trackWidth / 2)) * (60.0 / (M_PI * wheelDia)); |
akashvibhute | 10:ea4b90be68dc | 13 | *left_rpm = (velocity - (omega * trackWidth / 2)) * (60.0 / (M_PI * wheelDia)); |
akashvibhute | 10:ea4b90be68dc | 14 | |
akashvibhute | 10:ea4b90be68dc | 15 | /* |
akashvibhute | 10:ea4b90be68dc | 16 | if(*right_rpm > 100) |
akashvibhute | 10:ea4b90be68dc | 17 | *right_rpm = generalFunctions::constrain_f(*right_rpm, min_rpm, max_rpm); |
akashvibhute | 10:ea4b90be68dc | 18 | else |
akashvibhute | 10:ea4b90be68dc | 19 | *right_rpm = 0; |
akashvibhute | 10:ea4b90be68dc | 20 | |
akashvibhute | 10:ea4b90be68dc | 21 | if(*left_rpm > 100) |
akashvibhute | 10:ea4b90be68dc | 22 | *left_rpm = generalFunctions::constrain_f(*left_rpm, min_rpm, max_rpm); |
akashvibhute | 10:ea4b90be68dc | 23 | else |
akashvibhute | 10:ea4b90be68dc | 24 | *left_rpm = 0; |
akashvibhute | 10:ea4b90be68dc | 25 | */ |
akashvibhute | 10:ea4b90be68dc | 26 | } |