Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

motor_calibration.h@16:9c5ef6fe6780, 2018-10-31 (annotated)

Committer:
brass_phoenix
Date:
Wed Oct 31 13:45:46 2018 +0000
Revision:
16:9c5ef6fe6780
Child:
18:71a5cc9353af
+ UNTESTED: Added motor calibration.

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 16:9c5ef6fe6780 6 const double motor_calibration_threshold = 0.5*0.0174533;
brass_phoenix 16:9c5ef6fe6780 7
brass_phoenix 16:9c5ef6fe6780 8
brass_phoenix 16:9c5ef6fe6780 9 // Returns whether this motor is considered to be calibrated.
brass_phoenix 16:9c5ef6fe6780 10 bool calibrate_motor(Motor &motor, double &last_angle, int &iterations_not_moving) {
brass_phoenix 16:9c5ef6fe6780 11 float current_angle = motor.get_current_angle();
brass_phoenix 16:9c5ef6fe6780 12 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 13 {
brass_phoenix 16:9c5ef6fe6780 14 iterations_not_moving++;
brass_phoenix 16:9c5ef6fe6780 15 //calibLED1 = !calibLED1; // LED turns blue
brass_phoenix 16:9c5ef6fe6780 16 }
brass_phoenix 16:9c5ef6fe6780 17 else
brass_phoenix 16:9c5ef6fe6780 18 {
brass_phoenix 16:9c5ef6fe6780 19 double motor_angle = current_angle;
brass_phoenix 16:9c5ef6fe6780 20 motor_angle = motor_angle - 30 * motor_calibration_threshold; // Subtracting five degree angle from the current angle
brass_phoenix 16:9c5ef6fe6780 21 motor.set_target_angle(motor_angle);
brass_phoenix 16:9c5ef6fe6780 22 iterations_not_moving = 0; // Set time passed in this state back to zero
brass_phoenix 16:9c5ef6fe6780 23 //calibLED2 = !calibLED2; // LED turns red
brass_phoenix 16:9c5ef6fe6780 24 }
brass_phoenix 16:9c5ef6fe6780 25
brass_phoenix 16:9c5ef6fe6780 26 if (iterations_not_moving >= 10) // After 5 seconds have passed (10 times Ticker motor_calib)
brass_phoenix 16:9c5ef6fe6780 27 {
brass_phoenix 16:9c5ef6fe6780 28 return true;
brass_phoenix 16:9c5ef6fe6780 29 //alibLED3 = !calibLED3; // LED becomes red (purple if both motors are calibrated)
brass_phoenix 16:9c5ef6fe6780 30 }
brass_phoenix 16:9c5ef6fe6780 31
brass_phoenix 16:9c5ef6fe6780 32 last_angle = current_angle;
brass_phoenix 16:9c5ef6fe6780 33
brass_phoenix 16:9c5ef6fe6780 34 return false;
brass_phoenix 16:9c5ef6fe6780 35 }