blob: 784827ce217a9e7bf5b900359a6ced26df8580ea [file] [log] [blame]
Daniel Petti53b11092013-11-02 05:53:16 +00001#include <inttypes.h>
2
Daniel Petti3fe36542013-09-25 04:18:24 +00003#include "aos/atom_code/init.h"
4#include "aos/common/logging/logging.h"
Daniel Petti0663d302013-10-27 05:39:39 +00005#include "aos/common/util/wrapping_counter.h"
Daniel Petti3fe36542013-09-25 04:18:24 +00006
Daniel Petti1f448512013-10-19 19:35:55 +00007#include "bot3/control_loops/drivetrain/drivetrain.q.h"
James Kuszmaulf7f5ec12013-11-01 17:58:58 -07008#include "bot3/control_loops/shooter/shooter_motor.q.h"
Daniel Petti3fe36542013-09-25 04:18:24 +00009#include "frc971/queues/GyroAngle.q.h"
Daniel Petti0663d302013-10-27 05:39:39 +000010#include "frc971/input/usb_receiver.h"
Daniel Petti3fe36542013-09-25 04:18:24 +000011
12#ifndef M_PI
13#define M_PI 3.14159265358979323846
14#endif
15
Daniel Petti1f448512013-10-19 19:35:55 +000016using ::bot3::control_loops::drivetrain;
James Kuszmaulf7f5ec12013-11-01 17:58:58 -070017using ::bot3::control_loops::shooter;
Daniel Petti3fe36542013-09-25 04:18:24 +000018using ::frc971::sensors::gyro;
Daniel Petti0663d302013-10-27 05:39:39 +000019using ::aos::util::WrappingCounter;
20using ::frc971::USBReceiver;
Daniel Petti3fe36542013-09-25 04:18:24 +000021
Daniel Petti1f448512013-10-19 19:35:55 +000022namespace bot3 {
Daniel Petti3fe36542013-09-25 04:18:24 +000023namespace {
24
Daniel Petti03f58112013-11-09 16:35:55 +000025const double kWheelRadius = 1.964;
Daniel Pettiaba9c612013-11-14 10:17:35 -080026const double kTapeThickness = 3.12;
Daniel Petti03f58112013-11-09 16:35:55 +000027
Daniel Petti3fe36542013-09-25 04:18:24 +000028inline double drivetrain_translate(int32_t in) {
29 return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
Daniel Petti53b11092013-11-02 05:53:16 +000030 (32.0 / 44.0 /*encoder gears*/) * // the encoders are on the wheels.
Daniel Pettib046c532013-10-29 03:40:40 +000031 (3.5 /*wheel diameter*/ * 2.54 / 100 * M_PI);
Daniel Petti3fe36542013-09-25 04:18:24 +000032}
33
Daniel Petti44ad53a2013-11-04 05:47:52 +000034inline double shooter_translate(int32_t in) {
Daniel Petti5003b772013-11-07 02:19:50 +000035 return 1.0 / (static_cast<double>(in) / (10 ^ 8)/*ticks per sec*/)
Daniel Petti03f58112013-11-09 16:35:55 +000036 / (1 - (kTapeThickness / (2 * kWheelRadius * M_PI))) * (2 * M_PI);
Daniel Petti53b11092013-11-02 05:53:16 +000037}
38
39inline double gyro_translate(int64_t in) {
40 return in / 16.0 / 1000.0 / (180.0 / M_PI);
41}
42
Daniel Petti3fe36542013-09-25 04:18:24 +000043} // namespace
44
Daniel Petti0663d302013-10-27 05:39:39 +000045class GyroSensorReceiver : public USBReceiver {
Daniel Petti53b11092013-11-02 05:53:16 +000046 public:
47 GyroSensorReceiver() : USBReceiver(1) {}
Daniel Petti3fe36542013-09-25 04:18:24 +000048
Daniel Petti53b11092013-11-02 05:53:16 +000049 virtual void ProcessData(const ::aos::time::Time &/*timestamp*/) override {
Daniel Petti3fe36542013-09-25 04:18:24 +000050 gyro.MakeWithBuilder()
Daniel Petti53b11092013-11-02 05:53:16 +000051 .angle(gyro_translate(data()->gyro_angle))
Daniel Petti3fe36542013-09-25 04:18:24 +000052 .Send();
53
Daniel Petti53b11092013-11-02 05:53:16 +000054 LOG(DEBUG, "right drive: %f, left drive: %f\n",
55 drivetrain_translate(data()->main.shooter_angle),
56 drivetrain_translate(data()->main.indexer));
Daniel Petti3fe36542013-09-25 04:18:24 +000057 drivetrain.position.MakeWithBuilder()
James Kuszmaulf601eaa2013-10-31 06:25:09 -070058 .right_encoder(drivetrain_translate(data()->main.wrist))
59 .left_encoder(drivetrain_translate(data()->main.shooter))
Daniel Petti3fe36542013-09-25 04:18:24 +000060 .Send();
James Kuszmaulb74c8112013-11-03 16:13:45 -080061 /*drivetrain.position.MakeWithBuilder()
62 .right_encoder(0)
63 .left_encoder(0)
64 .Send();*/
Daniel Petti44ad53a2013-11-04 05:47:52 +000065
66 shooter.position.MakeWithBuilder()
Daniel Petti03f58112013-11-09 16:35:55 +000067 .velocity(shooter_translate(data()->bot3.shooter_cycle_ticks))
68 .Send();
Daniel Petti3fe36542013-09-25 04:18:24 +000069 }
Daniel Petti3fe36542013-09-25 04:18:24 +000070};
71
Daniel Petti1f448512013-10-19 19:35:55 +000072} // namespace bot3
Daniel Petti3fe36542013-09-25 04:18:24 +000073
74int main() {
Daniel Petti53b11092013-11-02 05:53:16 +000075 ::aos::Init(frc971::USBReceiver::kRelativePriority);
Daniel Petti0663d302013-10-27 05:39:39 +000076 ::bot3::GyroSensorReceiver receiver;
Daniel Petti3fe36542013-09-25 04:18:24 +000077 while (true) {
78 receiver.RunIteration();
79 }
80 ::aos::Cleanup();
81}