blob: f472d55339f8b66b8be9d4b7d52272e4ca1727d4 [file] [log] [blame]
Austin Schuha40624b2013-03-03 14:02:40 -08001#include "frc971/control_loops/wrist/wrist.h"
Austin Schuhdc1c84a2013-02-23 16:33:10 -08002
3#include <stdio.h>
4
5#include <algorithm>
6
Austin Schuhdc1c84a2013-02-23 16:33:10 -08007#include "aos/common/messages/RobotState.q.h"
8#include "aos/common/control_loop/control_loops.q.h"
9#include "aos/common/logging/logging.h"
10
11#include "frc971/constants.h"
Austin Schuha40624b2013-03-03 14:02:40 -080012#include "frc971/control_loops/wrist/wrist_motor_plant.h"
Austin Schuhdc1c84a2013-02-23 16:33:10 -080013
14namespace frc971 {
15namespace control_loops {
16
Austin Schuhdc1c84a2013-02-23 16:33:10 -080017WristMotor::WristMotor(control_loops::WristLoop *my_wrist)
18 : aos::control_loops::ControlLoop<control_loops::WristLoop>(my_wrist),
Austin Schuh844960d2013-03-09 17:07:51 -080019 zeroed_joint_(MakeWristLoop()) {
Austin Schuhdc1c84a2013-02-23 16:33:10 -080020}
21
Austin Schuh844960d2013-03-09 17:07:51 -080022bool WristMotor::FetchConstants(
23 ZeroedJoint<1>::ConfigurationData *config_data) {
24 if (!constants::wrist_lower_limit(&config_data->lower_limit)) {
James Kuszmaule06e2512013-03-02 15:04:53 -080025 LOG(ERROR, "Failed to fetch the wrist lower limit constant.\n");
Austin Schuhfa033692013-02-24 01:00:55 -080026 return false;
27 }
Austin Schuh844960d2013-03-09 17:07:51 -080028 if (!constants::wrist_upper_limit(&config_data->upper_limit)) {
James Kuszmaule06e2512013-03-02 15:04:53 -080029 LOG(ERROR, "Failed to fetch the wrist upper limit constant.\n");
Austin Schuhfa033692013-02-24 01:00:55 -080030 return false;
31 }
James Kuszmaule06e2512013-03-02 15:04:53 -080032 if (!constants::wrist_hall_effect_start_angle(
Austin Schuh844960d2013-03-09 17:07:51 -080033 &config_data->hall_effect_start_angle[0])) {
James Kuszmaule06e2512013-03-02 15:04:53 -080034 LOG(ERROR, "Failed to fetch the wrist start angle constant.\n");
Austin Schuhfa033692013-02-24 01:00:55 -080035 return false;
36 }
Austin Schuhdd3bc412013-03-16 17:02:40 -070037 if (!constants::wrist_zeroing_off_speed(&config_data->zeroing_off_speed)) {
38 LOG(ERROR, "Failed to fetch the wrist zeroing off speed constant.\n");
39 return false;
40 }
41
Austin Schuh844960d2013-03-09 17:07:51 -080042 if (!constants::wrist_zeroing_speed(&config_data->zeroing_speed)) {
James Kuszmaule06e2512013-03-02 15:04:53 -080043 LOG(ERROR, "Failed to fetch the wrist zeroing speed constant.\n");
Austin Schuhfa033692013-02-24 01:00:55 -080044 return false;
45 }
46
Austin Schuh844960d2013-03-09 17:07:51 -080047 config_data->max_zeroing_voltage = 5.0;
Austin Schuh039d4f92013-04-04 05:52:03 +000048 config_data->deadband_voltage = 0.0;
Austin Schuhfa033692013-02-24 01:00:55 -080049 return true;
50}
51
Austin Schuhfa033692013-02-24 01:00:55 -080052// Positive angle is up, and positive power is up.
Austin Schuhdc1c84a2013-02-23 16:33:10 -080053void WristMotor::RunIteration(
54 const ::aos::control_loops::Goal *goal,
55 const control_loops::WristLoop::Position *position,
56 ::aos::control_loops::Output *output,
Austin Schuh8b43caf2013-03-19 10:04:47 +000057 ::aos::control_loops::Status * status) {
Austin Schuhdc1c84a2013-02-23 16:33:10 -080058
Austin Schuhfa033692013-02-24 01:00:55 -080059 // Disable the motors now so that all early returns will return with the
60 // motors disabled.
Austin Schuhdc1c84a2013-02-23 16:33:10 -080061 if (output) {
62 output->voltage = 0;
63 }
64
Austin Schuhfa033692013-02-24 01:00:55 -080065 // Cache the constants to avoid error handling down below.
Austin Schuh844960d2013-03-09 17:07:51 -080066 ZeroedJoint<1>::ConfigurationData config_data;
67 if (!FetchConstants(&config_data)) {
Austin Schuhdc1c84a2013-02-23 16:33:10 -080068 return;
Austin Schuhdc1c84a2013-02-23 16:33:10 -080069 } else {
Austin Schuh844960d2013-03-09 17:07:51 -080070 zeroed_joint_.set_config_data(config_data);
Austin Schuhdc1c84a2013-02-23 16:33:10 -080071 }
72
Austin Schuh844960d2013-03-09 17:07:51 -080073 ZeroedJoint<1>::PositionData transformed_position;
74 ZeroedJoint<1>::PositionData *transformed_position_ptr =
75 &transformed_position;
76 if (!position) {
77 transformed_position_ptr = NULL;
Austin Schuhdc1c84a2013-02-23 16:33:10 -080078 } else {
Austin Schuh844960d2013-03-09 17:07:51 -080079 transformed_position.position = position->pos;
80 transformed_position.hall_effects[0] = position->hall_effect;
81 transformed_position.hall_effect_positions[0] = position->calibration;
Austin Schuhdc1c84a2013-02-23 16:33:10 -080082 }
83
Austin Schuh844960d2013-03-09 17:07:51 -080084 const double voltage = zeroed_joint_.Update(transformed_position_ptr,
85 output != NULL,
86 goal->goal, 0.0);
Austin Schuh06ee48e2013-03-02 01:47:54 -080087
Austin Schuhfa033692013-02-24 01:00:55 -080088 if (position) {
Brian Silverman18f6d872013-03-16 13:55:46 -070089 LOG(DEBUG, "pos: %f hall: %s absolute: %f\n",
90 position->pos,
91 position->hall_effect ? "true" : "false",
92 zeroed_joint_.absolute_position());
Austin Schuhfa033692013-02-24 01:00:55 -080093 }
94
Austin Schuhdc1c84a2013-02-23 16:33:10 -080095 if (output) {
Austin Schuh844960d2013-03-09 17:07:51 -080096 output->voltage = voltage;
Austin Schuhdc1c84a2013-02-23 16:33:10 -080097 }
Austin Schuh8b43caf2013-03-19 10:04:47 +000098 status->done = ::std::abs(zeroed_joint_.absolute_position() - goal->goal) < 0.004;
Austin Schuhdc1c84a2013-02-23 16:33:10 -080099}
100
101} // namespace control_loops
102} // namespace frc971