Ion Systems / Mbed 2 deprecated triforce-control

Control code for Triforce robot.

Dependencies:   triforce-esc PwmInRC mbed

Triforce.h@2:4e086a77e769, 2017-05-05 (annotated)

Committer:
IonSystems
Date:
Fri May 05 14:50:05 2017 +0000
Revision:
2:4e086a77e769
Parent:
1:026f79cfd378 
Forked ESC library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IonSystems 1:026f79cfd378 1 #ifndef TRIFORCE_H
IonSystems 1:026f79cfd378 2 #define TRIFORCE_H
IonSystems 0:d6eeeae3c3cb 3
IonSystems 1:026f79cfd378 4 /******************************************************************************/
IonSystems 1:026f79cfd378 5
IonSystems 1:026f79cfd378 6 /* Debugging defines */
IonSystems 1:026f79cfd378 7 #define DEBUG_INPUT // Send input values to usb serial
IonSystems 1:026f79cfd378 8 //#define DEBUG_ORIENTATION // Send orientation readings to usb serial
IonSystems 1:026f79cfd378 9 //#define DEBUG_HEADING_LOCK // Send heading lock values to usb serial
IonSystems 1:026f79cfd378 10 //#define DEBUG_CONTROLS // Send calculated controls to usb serial
IonSystems 1:026f79cfd378 11 #define DEBUG_OUTPUT // Send output values to usb serial
IonSystems 1:026f79cfd378 12 #define PC_DEBUGGING // Output debugging information over usb serial
IonSystems 1:026f79cfd378 13
IonSystems 1:026f79cfd378 14 /* Configuration defines */
IonSystems 1:026f79cfd378 15 #define HEADING_LOCK // Enable heading lock functionality
IonSystems 1:026f79cfd378 16 #define ORIENTATION // Enable orientation functionality
IonSystems 1:026f79cfd378 17 #define OMNI_MIXER // Enable omni mixer functionality
IonSystems 1:026f79cfd378 18
IonSystems 1:026f79cfd378 19 #define channel_max 85 //85
IonSystems 1:026f79cfd378 20 #define channel_min 3 //3
IonSystems 1:026f79cfd378 21 #define RC_ON_THRES 70 // ON threshold for switches
IonSystems 1:026f79cfd378 22 #define RC_OFF_THRES 30 // OFF threshold for switches
IonSystems 1:026f79cfd378 23 #define RC_FAILSAFE_THRES 150 // FAILSAFE threshold
IonSystems 1:026f79cfd378 24
IonSystems 1:026f79cfd378 25
IonSystems 1:026f79cfd378 26 /******************************************************************************/
IonSystems 1:026f79cfd378 27
IonSystems 1:026f79cfd378 28 #include "math.h"
IonSystems 1:026f79cfd378 29
IonSystems 1:026f79cfd378 30 /******************************************************************************/
IonSystems 0:d6eeeae3c3cb 31
IonSystems 0:d6eeeae3c3cb 32 /* Positions of controls */
IonSystems 0:d6eeeae3c3cb 33 struct rc_controls{
IonSystems 0:d6eeeae3c3cb 34 int channel_1;
IonSystems 0:d6eeeae3c3cb 35 int channel_2;
IonSystems 0:d6eeeae3c3cb 36 int channel_3;
IonSystems 0:d6eeeae3c3cb 37 int channel_4;
IonSystems 0:d6eeeae3c3cb 38 int channel_5;
IonSystems 0:d6eeeae3c3cb 39 int channel_6;
IonSystems 0:d6eeeae3c3cb 40 int channel_7;
IonSystems 0:d6eeeae3c3cb 41 int channel_8;
IonSystems 0:d6eeeae3c3cb 42 };
IonSystems 0:d6eeeae3c3cb 43
IonSystems 0:d6eeeae3c3cb 44 /* Current movement command */
IonSystems 0:d6eeeae3c3cb 45 struct direction_vector{
IonSystems 0:d6eeeae3c3cb 46 float rotation;
IonSystems 0:d6eeeae3c3cb 47 float x_translation;
IonSystems 0:d6eeeae3c3cb 48 float y_translation;
IonSystems 0:d6eeeae3c3cb 49 };
IonSystems 0:d6eeeae3c3cb 50
IonSystems 0:d6eeeae3c3cb 51 /* Outputs for devices */
IonSystems 0:d6eeeae3c3cb 52 struct rc_outputs{
IonSystems 0:d6eeeae3c3cb 53 int wheel_1;
IonSystems 0:d6eeeae3c3cb 54 int wheel_2;
IonSystems 1:026f79cfd378 55 int wheel_3;
IonSystems 0:d6eeeae3c3cb 56 int weapon_motor_1;
IonSystems 0:d6eeeae3c3cb 57 int weapon_motor_2;
IonSystems 0:d6eeeae3c3cb 58 };
IonSystems 0:d6eeeae3c3cb 59
IonSystems 0:d6eeeae3c3cb 60 /* Ring definition */
IonSystems 0:d6eeeae3c3cb 61 struct ring{
IonSystems 0:d6eeeae3c3cb 62 const int max_rpm;
IonSystems 0:d6eeeae3c3cb 63 int rpm;
IonSystems 0:d6eeeae3c3cb 64 const int max_energy;
IonSystems 0:d6eeeae3c3cb 65 float energy;
IonSystems 0:d6eeeae3c3cb 66 };
IonSystems 0:d6eeeae3c3cb 67
IonSystems 0:d6eeeae3c3cb 68 /* Battery definition */
IonSystems 0:d6eeeae3c3cb 69 struct battery{
IonSystems 0:d6eeeae3c3cb 70 int capacity;
IonSystems 0:d6eeeae3c3cb 71 int used_capacity;
IonSystems 0:d6eeeae3c3cb 72 const float max_voltage;
IonSystems 0:d6eeeae3c3cb 73 float voltage;
IonSystems 0:d6eeeae3c3cb 74 const int max_amps;
IonSystems 0:d6eeeae3c3cb 75 int amps;
IonSystems 0:d6eeeae3c3cb 76 const int max_temperature;
IonSystems 0:d6eeeae3c3cb 77 int temperature;
IonSystems 0:d6eeeae3c3cb 78 };
IonSystems 0:d6eeeae3c3cb 79
IonSystems 0:d6eeeae3c3cb 80 /* Motor definition */
IonSystems 0:d6eeeae3c3cb 81 struct motor{
IonSystems 0:d6eeeae3c3cb 82 const int max_rpm;
IonSystems 0:d6eeeae3c3cb 83 int rpm;
IonSystems 0:d6eeeae3c3cb 84 const int max_power;
IonSystems 0:d6eeeae3c3cb 85 int power;
IonSystems 0:d6eeeae3c3cb 86 const float max_voltage;
IonSystems 0:d6eeeae3c3cb 87 float voltage;
IonSystems 0:d6eeeae3c3cb 88 const float max_amps;
IonSystems 0:d6eeeae3c3cb 89 float amps;
IonSystems 0:d6eeeae3c3cb 90 const int max_temperature;
IonSystems 0:d6eeeae3c3cb 91 int temperature;
IonSystems 0:d6eeeae3c3cb 92 };
IonSystems 0:d6eeeae3c3cb 93
IonSystems 0:d6eeeae3c3cb 94 /* Distance sensor definition */
IonSystems 0:d6eeeae3c3cb 95 struct distance_sensor{
IonSystems 0:d6eeeae3c3cb 96 int id;
IonSystems 0:d6eeeae3c3cb 97 const float max_range;
IonSystems 0:d6eeeae3c3cb 98 float range;
IonSystems 0:d6eeeae3c3cb 99 };
IonSystems 0:d6eeeae3c3cb 100
IonSystems 1:026f79cfd378 101 /******************************************************************************/
IonSystems 1:026f79cfd378 102
IonSystems 1:026f79cfd378 103 /* Convert from pulsewidth in seconds, to % */
IonSystems 1:026f79cfd378 104 int convert_pulsewidth(float pulsewidth){
IonSystems 1:026f79cfd378 105 //return(int) ((pulsewidth -1000) / 10.0f);
IonSystems 1:026f79cfd378 106 float value = (float) ((pulsewidth -1000) / 10.0f);
IonSystems 1:026f79cfd378 107 int pulse = (((value-channel_min)/channel_max) * 100.0);
IonSystems 1:026f79cfd378 108 return clamp(pulse, 0, 100);
IonSystems 1:026f79cfd378 109 }
IonSystems 1:026f79cfd378 110
IonSystems 1:026f79cfd378 111 float normalize(float heading){
IonSystems 1:026f79cfd378 112 while (heading > 180)
IonSystems 1:026f79cfd378 113 heading -= 360;
IonSystems 1:026f79cfd378 114 while (heading < -180)
IonSystems 1:026f79cfd378 115 heading += 360;
IonSystems 1:026f79cfd378 116 return heading;
IonSystems 0:d6eeeae3c3cb 117 }
IonSystems 1:026f79cfd378 118
IonSystems 1:026f79cfd378 119 /******************************************************************************/
IonSystems 1:026f79cfd378 120
IonSystems 1:026f79cfd378 121 /* Orientation */
IonSystems 1:026f79cfd378 122 Euler orientation;
IonSystems 1:026f79cfd378 123
IonSystems 1:026f79cfd378 124 /* Is the robot inverted */
IonSystems 1:026f79cfd378 125 bool inverted = false;
IonSystems 1:026f79cfd378 126
IonSystems 1:026f79cfd378 127 bool heading_lock_enabled = false;
IonSystems 1:026f79cfd378 128 float heading_lock = 0; // heading to rotate to
IonSystems 1:026f79cfd378 129 int heading_lock_speed = 10; // percentage
IonSystems 1:026f79cfd378 130 int heading_lock_deadband = 10; // degrees / 2
IonSystems 1:026f79cfd378 131
IonSystems 1:026f79cfd378 132 bool failsafe = false;
IonSystems 1:026f79cfd378 133 const int NO_SIGNAL_TIMEOUT = 70; // 150ms increments
IonSystems 1:026f79cfd378 134 int failsafe_timer = 0;
IonSystems 1:026f79cfd378 135
IonSystems 1:026f79cfd378 136 bool armed = false;
IonSystems 1:026f79cfd378 137 bool just_armed = false;
IonSystems 1:026f79cfd378 138
IonSystems 1:026f79cfd378 139 int arming_count = 0;
IonSystems 1:026f79cfd378 140 const int RC_ARM_CHANNEL_1 = 90;
IonSystems 1:026f79cfd378 141 const int RC_ARM_CHANNEL_2 = 90;
IonSystems 1:026f79cfd378 142 const int RC_ARM_CHANNEL_3 = 10;
IonSystems 1:026f79cfd378 143 const int RC_ARM_CHANNEL_4 = 10;
IonSystems 1:026f79cfd378 144 const int RC_ARM_DELAY = 5;
IonSystems 1:026f79cfd378 145
IonSystems 1:026f79cfd378 146
IonSystems 1:026f79cfd378 147 /******************************************************************************/
IonSystems 1:026f79cfd378 148
IonSystems 1:026f79cfd378 149 #endif