State machine
Dependencies: mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter
motor_calibration.h@51:e0e4d7e3de93, 2018-11-05 (annotated)
- Committer:
- MAHCSnijders
- Date:
- Mon Nov 05 16:03:39 2018 +0000
- Revision:
- 51:e0e4d7e3de93
- Parent:
- 33:543debddb3a9
Fixed comments
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
brass_phoenix | 16:9c5ef6fe6780 | 1 | #pragma once |
brass_phoenix | 16:9c5ef6fe6780 | 2 | |
brass_phoenix | 16:9c5ef6fe6780 | 3 | #include "mbed.h" |
brass_phoenix | 16:9c5ef6fe6780 | 4 | #include "motor.h" |
brass_phoenix | 16:9c5ef6fe6780 | 5 | |
brass_phoenix | 18:71a5cc9353af | 6 | const double motor_rad_per_angle = 0.0174533; |
brass_phoenix | 18:71a5cc9353af | 7 | const double motor_calibration_threshold = 0.1*motor_rad_per_angle; |
brass_phoenix | 16:9c5ef6fe6780 | 8 | |
brass_phoenix | 16:9c5ef6fe6780 | 9 | |
brass_phoenix | 16:9c5ef6fe6780 | 10 | // Returns whether this motor is considered to be calibrated. |
brass_phoenix | 16:9c5ef6fe6780 | 11 | bool calibrate_motor(Motor &motor, double &last_angle, int &iterations_not_moving) { |
brass_phoenix | 16:9c5ef6fe6780 | 12 | float current_angle = motor.get_current_angle(); |
brass_phoenix | 16:9c5ef6fe6780 | 13 | if (current_angle - motor_calibration_threshold <= last_angle && last_angle <= current_angle + motor_calibration_threshold) // If the motor angle is within a margin of the current motor angle |
brass_phoenix | 16:9c5ef6fe6780 | 14 | { |
brass_phoenix | 16:9c5ef6fe6780 | 15 | iterations_not_moving++; |
brass_phoenix | 16:9c5ef6fe6780 | 16 | //calibLED1 = !calibLED1; // LED turns blue |
brass_phoenix | 16:9c5ef6fe6780 | 17 | } |
brass_phoenix | 16:9c5ef6fe6780 | 18 | else |
brass_phoenix | 16:9c5ef6fe6780 | 19 | { |
brass_phoenix | 16:9c5ef6fe6780 | 20 | double motor_angle = current_angle; |
brass_phoenix | 28:25917b26022c | 21 | motor_angle = motor_angle - 5 * motor_rad_per_angle; // Subtracting five degree angle from the current angle |
brass_phoenix | 16:9c5ef6fe6780 | 22 | motor.set_target_angle(motor_angle); |
brass_phoenix | 16:9c5ef6fe6780 | 23 | iterations_not_moving = 0; // Set time passed in this state back to zero |
brass_phoenix | 16:9c5ef6fe6780 | 24 | //calibLED2 = !calibLED2; // LED turns red |
brass_phoenix | 16:9c5ef6fe6780 | 25 | } |
brass_phoenix | 16:9c5ef6fe6780 | 26 | |
brass_phoenix | 33:543debddb3a9 | 27 | if (iterations_not_moving >= 10) // After enough time has passed. |
brass_phoenix | 16:9c5ef6fe6780 | 28 | { |
brass_phoenix | 16:9c5ef6fe6780 | 29 | return true; |
brass_phoenix | 16:9c5ef6fe6780 | 30 | //alibLED3 = !calibLED3; // LED becomes red (purple if both motors are calibrated) |
brass_phoenix | 16:9c5ef6fe6780 | 31 | } |
brass_phoenix | 16:9c5ef6fe6780 | 32 | |
brass_phoenix | 16:9c5ef6fe6780 | 33 | last_angle = current_angle; |
brass_phoenix | 16:9c5ef6fe6780 | 34 | |
brass_phoenix | 16:9c5ef6fe6780 | 35 | return false; |
brass_phoenix | 16:9c5ef6fe6780 | 36 | } |