Biorobotics 7 / Mbed 2 deprecated State_Machine

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?

UserRevisionLine numberNew 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 }