Ion Systems / Mbed 2 deprecated triforce-control

Control code for Triforce robot.

Dependencies:   triforce-esc PwmInRC mbed

Triforce.h@0:d6eeeae3c3cb, 2016-11-18 (annotated)

Committer:
IonSystems
Date:
Fri Nov 18 14:22:26 2016 +0000
Revision:
0:d6eeeae3c3cb
Child:
1:026f79cfd378
Working input and bno055 reading. Working on omnimixing.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IonSystems 0:d6eeeae3c3cb 1
IonSystems 0:d6eeeae3c3cb 2 float channel_max = 85;
IonSystems 0:d6eeeae3c3cb 3 float channel_min = 3;
IonSystems 0:d6eeeae3c3cb 4
IonSystems 0:d6eeeae3c3cb 5 /* Positions of controls */
IonSystems 0:d6eeeae3c3cb 6 struct rc_controls{
IonSystems 0:d6eeeae3c3cb 7 int channel_1;
IonSystems 0:d6eeeae3c3cb 8 int channel_2;
IonSystems 0:d6eeeae3c3cb 9 int channel_3;
IonSystems 0:d6eeeae3c3cb 10 int channel_4;
IonSystems 0:d6eeeae3c3cb 11 int channel_5;
IonSystems 0:d6eeeae3c3cb 12 int channel_6;
IonSystems 0:d6eeeae3c3cb 13 int channel_7;
IonSystems 0:d6eeeae3c3cb 14 int channel_8;
IonSystems 0:d6eeeae3c3cb 15 };
IonSystems 0:d6eeeae3c3cb 16
IonSystems 0:d6eeeae3c3cb 17 /* Current movement command */
IonSystems 0:d6eeeae3c3cb 18 struct direction_vector{
IonSystems 0:d6eeeae3c3cb 19 float rotation;
IonSystems 0:d6eeeae3c3cb 20 float x_translation;
IonSystems 0:d6eeeae3c3cb 21 float y_translation;
IonSystems 0:d6eeeae3c3cb 22 };
IonSystems 0:d6eeeae3c3cb 23
IonSystems 0:d6eeeae3c3cb 24 /* Outputs for devices */
IonSystems 0:d6eeeae3c3cb 25 struct rc_outputs{
IonSystems 0:d6eeeae3c3cb 26 int wheel_1;
IonSystems 0:d6eeeae3c3cb 27 int wheel_2;
IonSystems 0:d6eeeae3c3cb 28 int wheel_3;
IonSystems 0:d6eeeae3c3cb 29 int weapon_motor_1;
IonSystems 0:d6eeeae3c3cb 30 int weapon_motor_2;
IonSystems 0:d6eeeae3c3cb 31 };
IonSystems 0:d6eeeae3c3cb 32
IonSystems 0:d6eeeae3c3cb 33 /* Convert from pulsewidth in seconds, to % */
IonSystems 0:d6eeeae3c3cb 34 int convert_pulsewidth(float pulsewidth){
IonSystems 0:d6eeeae3c3cb 35 //return(int) ((pulsewidth -1000) / 10.0f);
IonSystems 0:d6eeeae3c3cb 36 float value = (float) ((pulsewidth -1000) / 10.0f);
IonSystems 0:d6eeeae3c3cb 37 return (int) (((value-channel_min)/channel_max) * 100.0);
IonSystems 0:d6eeeae3c3cb 38 }
IonSystems 0:d6eeeae3c3cb 39
IonSystems 0:d6eeeae3c3cb 40 float normalize(float heading){
IonSystems 0:d6eeeae3c3cb 41 while (heading > 180)
IonSystems 0:d6eeeae3c3cb 42 heading -= 360;
IonSystems 0:d6eeeae3c3cb 43 while (heading < -180)
IonSystems 0:d6eeeae3c3cb 44 heading += 360;
IonSystems 0:d6eeeae3c3cb 45 return heading;
IonSystems 0:d6eeeae3c3cb 46 }
IonSystems 0:d6eeeae3c3cb 47
IonSystems 0:d6eeeae3c3cb 48 /* Orientation */
IonSystems 0:d6eeeae3c3cb 49 Euler orientation;
IonSystems 0:d6eeeae3c3cb 50
IonSystems 0:d6eeeae3c3cb 51 /* Is the robot inverted */
IonSystems 0:d6eeeae3c3cb 52 bool inverted = false;
IonSystems 0:d6eeeae3c3cb 53
IonSystems 0:d6eeeae3c3cb 54 /* Heading to rotate to with heading lock enabled */
IonSystems 0:d6eeeae3c3cb 55 float heading_lock = 0;
IonSystems 0:d6eeeae3c3cb 56
IonSystems 0:d6eeeae3c3cb 57 /* Ring definition */
IonSystems 0:d6eeeae3c3cb 58 struct ring{
IonSystems 0:d6eeeae3c3cb 59 const int max_rpm;
IonSystems 0:d6eeeae3c3cb 60 int rpm;
IonSystems 0:d6eeeae3c3cb 61 const int max_energy;
IonSystems 0:d6eeeae3c3cb 62 float energy;
IonSystems 0:d6eeeae3c3cb 63 };
IonSystems 0:d6eeeae3c3cb 64
IonSystems 0:d6eeeae3c3cb 65 /* Battery definition */
IonSystems 0:d6eeeae3c3cb 66 struct battery{
IonSystems 0:d6eeeae3c3cb 67 int capacity;
IonSystems 0:d6eeeae3c3cb 68 int used_capacity;
IonSystems 0:d6eeeae3c3cb 69 const float max_voltage;
IonSystems 0:d6eeeae3c3cb 70 float voltage;
IonSystems 0:d6eeeae3c3cb 71 const int max_amps;
IonSystems 0:d6eeeae3c3cb 72 int amps;
IonSystems 0:d6eeeae3c3cb 73 const int max_temperature;
IonSystems 0:d6eeeae3c3cb 74 int temperature;
IonSystems 0:d6eeeae3c3cb 75 };
IonSystems 0:d6eeeae3c3cb 76
IonSystems 0:d6eeeae3c3cb 77 /* Motor definition */
IonSystems 0:d6eeeae3c3cb 78 struct motor{
IonSystems 0:d6eeeae3c3cb 79 const int max_rpm;
IonSystems 0:d6eeeae3c3cb 80 int rpm;
IonSystems 0:d6eeeae3c3cb 81 const int max_power;
IonSystems 0:d6eeeae3c3cb 82 int power;
IonSystems 0:d6eeeae3c3cb 83 const float max_voltage;
IonSystems 0:d6eeeae3c3cb 84 float voltage;
IonSystems 0:d6eeeae3c3cb 85 const float max_amps;
IonSystems 0:d6eeeae3c3cb 86 float amps;
IonSystems 0:d6eeeae3c3cb 87 const int max_temperature;
IonSystems 0:d6eeeae3c3cb 88 int temperature;
IonSystems 0:d6eeeae3c3cb 89 };
IonSystems 0:d6eeeae3c3cb 90
IonSystems 0:d6eeeae3c3cb 91 /* Distance sensor definition */
IonSystems 0:d6eeeae3c3cb 92 struct distance_sensor{
IonSystems 0:d6eeeae3c3cb 93 int id;
IonSystems 0:d6eeeae3c3cb 94 const float max_range;
IonSystems 0:d6eeeae3c3cb 95 float range;
IonSystems 0:d6eeeae3c3cb 96 };
IonSystems 0:d6eeeae3c3cb 97
IonSystems 0:d6eeeae3c3cb 98 float map(float in, float inMin, float inMax, float outMin, float outMax) {
IonSystems 0:d6eeeae3c3cb 99 // check it's within the range
IonSystems 0:d6eeeae3c3cb 100 if (inMin<inMax) {
IonSystems 0:d6eeeae3c3cb 101 if (in <= inMin)
IonSystems 0:d6eeeae3c3cb 102 return outMin;
IonSystems 0:d6eeeae3c3cb 103 if (in >= inMax)
IonSystems 0:d6eeeae3c3cb 104 return outMax;
IonSystems 0:d6eeeae3c3cb 105 } else { // cope with input range being backwards.
IonSystems 0:d6eeeae3c3cb 106 if (in >= inMin)
IonSystems 0:d6eeeae3c3cb 107 return outMin;
IonSystems 0:d6eeeae3c3cb 108 if (in <= inMax)
IonSystems 0:d6eeeae3c3cb 109 return outMax;
IonSystems 0:d6eeeae3c3cb 110 }
IonSystems 0:d6eeeae3c3cb 111 // calculate how far into the range we are
IonSystems 0:d6eeeae3c3cb 112 float scale = (in-inMin)/(inMax-inMin);
IonSystems 0:d6eeeae3c3cb 113 // calculate the output.
IonSystems 0:d6eeeae3c3cb 114 return outMin + scale*(outMax-outMin);
IonSystems 0:d6eeeae3c3cb 115 }
IonSystems 0:d6eeeae3c3cb 116
IonSystems 0:d6eeeae3c3cb 117 float clamp(float d, float min, float max) {
IonSystems 0:d6eeeae3c3cb 118 const float t = d < min ? min : d;
IonSystems 0:d6eeeae3c3cb 119 return t > max ? max : t;
IonSystems 0:d6eeeae3c3cb 120 }