Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

motor_calibration.h@28:25917b26022c, 2018-11-01 (annotated)

Committer:
brass_phoenix
Date:
Thu Nov 01 12:31:39 2018 +0000
Revision:
28:25917b26022c
Parent:
19:53b9729fbab5
Child:
29:77fee8a01529
+ Added debug for 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 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 19:53b9729fbab5 27 if (iterations_not_moving >= 40) // After 5 seconds have passed (10 times Ticker motor_calib)
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 }