State machine
Dependencies: mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter
motor_calibration.h
- Committer:
- MAHCSnijders
- Date:
- 2018-11-05
- Revision:
- 51:e0e4d7e3de93
- Parent:
- 33:543debddb3a9
File content as of revision 51:e0e4d7e3de93:
#pragma once #include "mbed.h" #include "motor.h" const double motor_rad_per_angle = 0.0174533; const double motor_calibration_threshold = 0.1*motor_rad_per_angle; // Returns whether this motor is considered to be calibrated. bool calibrate_motor(Motor &motor, double &last_angle, int &iterations_not_moving) { float current_angle = motor.get_current_angle(); 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 { iterations_not_moving++; //calibLED1 = !calibLED1; // LED turns blue } else { double motor_angle = current_angle; motor_angle = motor_angle - 5 * motor_rad_per_angle; // Subtracting five degree angle from the current angle motor.set_target_angle(motor_angle); iterations_not_moving = 0; // Set time passed in this state back to zero //calibLED2 = !calibLED2; // LED turns red } if (iterations_not_moving >= 10) // After enough time has passed. { return true; //alibLED3 = !calibLED3; // LED becomes red (purple if both motors are calibrated) } last_angle = current_angle; return false; }