Ion Systems
Control code for Triforce robot.
Dependencies: triforce-esc PwmInRC mbed
Diff: main.cpp
- Revision:
- 1:026f79cfd378
- Parent:
- 0:d6eeeae3c3cb
--- a/main.cpp Fri Nov 18 14:22:26 2016 +0000
+++ b/main.cpp Fri May 05 14:12:24 2017 +0000
@@ -1,47 +1,33 @@
-/* Debugging defines */
-#define DEBUG_INPUT // Send input values to usb serial
-#define DEBUG_ORIENTATION // Send orientation readings to usb serial
-#define DEBUG_OUTPUT // Send output values to usb serial
-#define PC_DEBUGGING // Output debugging information over usb serial
-
-/* Configuration defines */
-//#define HEADING_LOCK // Enable heading lock functionality
-//#define ORIENTATION // Enable orientation functionality
/* Includes */
-
#include "bno055.h"
#include "esc.h"
#include "PwmIn.h"
#include "mbed.h"
#include "Triforce.h"
-
-/* Set the delay between fresh samples */
-#define BNO055_SAMPLERATE_DELAY_MS (100)
+#include "math.h"
/* USB */
Serial pc(USBTX, USBRX);
/* 8 Channel RC input */
-PwmIn rc_channel_1(p5);
-PwmIn rc_channel_2(p6);
-PwmIn rc_channel_3(p7);
-PwmIn rc_channel_4(p8);
-PwmIn rc_channel_5(p11);
-PwmIn rc_channel_6(p12);
-PwmIn rc_channel_7(p13);
-PwmIn rc_channel_8(p14);
+PwmIn rc_channel_1(p5); // Roll
+PwmIn rc_channel_2(p6); // Pitch
+PwmIn rc_channel_3(p7); // Throttle
+PwmIn rc_channel_4(p8); // Yaw
+PwmIn rc_channel_5(p11); // Switch R front
+PwmIn rc_channel_6(p12); // Dial R
+PwmIn rc_channel_7(p13); // Dial L
+PwmIn rc_channel_8(p14); // Switch L Back
/* 5 channel ESC output */
-ESC esc_omni_1(p21);
-ESC esc_omni_2(p22);
-ESC esc_omni_3(p23); //LED4
-//ESC esc_weapon_1(p24); //LED3
-//ESC esc_weapon_2(p25); //LED2
+ESC esc_omni_1(p21, 20, 1500);
+ESC esc_omni_2(p22, 20, 1500);
+ESC esc_omni_3(p23, 20, 1500); //LED4
+ESC esc_weapon_1(p24); //LED3
+ESC esc_weapon_2(p25); //LED2
/* LEDs */
DigitalOut led1(LED1);
-DigitalOut led2(LED2);
-DigitalOut led3(LED3);
/* Initial control positions */
static struct rc_controls controls = {
@@ -71,41 +57,9 @@
.y_translation = 50
};
-/** Read control positions from RC receiver
- *
- * Converts control input pulsewidths read in seconds to throttle %
- */
-void read_inputs(){
- controls.channel_1 = convert_pulsewidth(rc_channel_1.pulsewidth());
- controls.channel_2 = convert_pulsewidth(rc_channel_2.pulsewidth());
- controls.channel_3 = convert_pulsewidth(rc_channel_3.pulsewidth());
- controls.channel_4 = convert_pulsewidth(rc_channel_4.pulsewidth());
- controls.channel_5 = convert_pulsewidth(rc_channel_5.pulsewidth());
- controls.channel_6 = convert_pulsewidth(rc_channel_6.pulsewidth());
- controls.channel_7 = convert_pulsewidth(rc_channel_7.pulsewidth());
- controls.channel_8 = convert_pulsewidth(rc_channel_8.pulsewidth());
-
- #if defined (PC_DEBUGGING) && defined (DEBUG_INPUT)
- pc.printf("Input: C1: %i, C2: %i, C3: %i, C4: %i, C5: %i, C6: %i, C7: %i, C8: %i\r\n", controls.channel_1, controls.channel_2, controls.channel_3, controls.channel_4, controls.channel_5, controls.channel_6, controls.channel_7, controls.channel_8);
- #endif
-}
+/******************************************************************************/
-/** Output to OmniMixer and ESCs
- *
- * Sends calculated output values to devices
- */
-void output_stage(){
- esc_omni_1.setThrottle(outputs.wheel_1);
- esc_omni_2.setThrottle(outputs.wheel_2);
- esc_omni_3.setThrottle(outputs.wheel_3);
- //esc_weapon_1.setThrottle(outputs.weapon_motor_1);
- //esc_weapon_2.setThrottle(outputs.weapon_motor_2);
-
- #if defined (PC_DEBUGGING) && defined (DEBUG_OUTPUT)
- pc.printf("Output Stage: Rotation: %i, X: %i, Y: %i, Weapon1: %i, Weapon2: %i", outputs.wheel_1, outputs.wheel_2, outputs.wheel_3, outputs.weapon_motor_1, outputs.weapon_motor_2);
- #endif
-}
/** Init
*
@@ -132,6 +86,133 @@
#endif
}
+void reset_outputs(){
+ outputs.wheel_1 = 50;
+ outputs.wheel_2 = 50;
+ outputs.wheel_3 = 50;
+}
+
+
+/** Read control positions from RC receiver
+ *
+ * Converts control input pulsewidths read in seconds to throttle %
+ */
+void read_inputs(){
+
+
+ /* Check for armed status */
+
+
+ if(!armed || just_armed){
+ #if defined (PC_DEBUGGING) && defined (DEBUG_INPUT)
+ if(arming_count == 0){
+
+ pc.printf("MOVE STICKS TO ARMING POSITIONS AND HOLD TO ARM \r\n");
+ }
+ #endif
+
+ failsafe = true;
+ int c1 = convert_pulsewidth(rc_channel_1.pulsewidth());
+ int c2 = convert_pulsewidth(rc_channel_2.pulsewidth());
+ int c3 = convert_pulsewidth(rc_channel_3.pulsewidth());
+ int c4 = convert_pulsewidth(rc_channel_4.pulsewidth());
+ pc.printf("Input: C1: %i \t C2: %i \t C3: %i \t C4: %i \r\n", c1, c2, c3, c4);
+
+ if(c1 > RC_ARM_CHANNEL_1 && c2 > RC_ARM_CHANNEL_2 && c3 < RC_ARM_CHANNEL_3 && c4 < RC_ARM_CHANNEL_4){
+
+ if(!armed){
+ #if defined (PC_DEBUGGING) && defined (DEBUG_INPUT)
+ pc.printf("ARMING %i\r\n", arming_count);
+ #endif
+ arming_count++;
+ wait_ms(1000);
+ if(arming_count > RC_ARM_DELAY){
+ armed = true;
+ just_armed = true;
+ }
+ }else{
+ #if defined (PC_DEBUGGING) && defined (DEBUG_INPUT)
+ pc.printf("ARMED - CENTER CONTROLS \r\n");
+ #endif
+ }
+ } else if(just_armed){
+ /* Wait for controls to be centered to avoid unexpected movement */
+ if(BETWEEN(c1, 45, 55) && BETWEEN(c2, 45, 55) && c3 < RC_ARM_CHANNEL_3 && BETWEEN(c4, 45, 55)){
+ #if defined (PC_DEBUGGING) && defined (DEBUG_INPUT)
+ pc.printf("CONTROLS CENTERED DISABLING ARM LOCK\r\n");
+ #endif
+ just_armed = false;
+ failsafe_timer = 0;
+ }
+
+ }else {
+ arming_count = 0;
+ }
+
+
+ }else{
+
+
+
+
+
+ #if defined (PC_DEBUGGING) && defined (DEBUG_INPUT)
+ pc.printf("******************ARMED****************** \r\n");
+ #endif
+
+ controls.channel_1 = convert_pulsewidth(rc_channel_1.pulsewidth());
+ controls.channel_2 = convert_pulsewidth(rc_channel_2.pulsewidth());
+ controls.channel_3 = convert_pulsewidth(rc_channel_3.pulsewidth());
+ controls.channel_4 = convert_pulsewidth(rc_channel_4.pulsewidth());
+ controls.channel_5 = convert_pulsewidth(rc_channel_5.pulsewidth());
+ controls.channel_6 = convert_pulsewidth(rc_channel_6.pulsewidth());
+ controls.channel_7 = convert_pulsewidth(rc_channel_7.pulsewidth());
+ controls.channel_8 = convert_pulsewidth(rc_channel_8.pulsewidth());
+
+ /* Check if rc is stalled and failsafe output */
+
+ if(rc_channel_1.stallTimer.read_ms() > 200){
+ failsafe = true;
+ #if defined (PC_DEBUGGING) && defined (DEBUG_INPUT)
+ pc.printf("NO SIGNAL - FAILSAFE ACTIVATED \r\n");
+ #endif
+ failsafe_timer++;
+ wait_ms(100);
+ if(failsafe_timer > NO_SIGNAL_TIMEOUT){
+ armed = false;
+ }
+ }else{
+ failsafe_timer = 0;
+ failsafe = false;
+ }
+
+
+
+ direction.rotation = controls.channel_4;
+ direction.x_translation = controls.channel_2;
+ direction.y_translation = controls.channel_1;
+
+ /* Heading lock enable switch */
+ if(controls.channel_8 > 75 && !heading_lock_enabled){
+ heading_lock = orientation.heading;
+ heading_lock_enabled = true;
+ }else if(controls.channel_8 < 75 && heading_lock_enabled){
+ heading_lock_enabled = false;
+ }
+
+ /* Speed for heading lock */
+ heading_lock_speed = (int) controls.channel_7;
+
+ #if defined (PC_DEBUGGING) && defined (DEBUG_INPUT)
+ pc.printf("Input: C1: %i \t C2: %i \t C3: %i \t C4: %i \t C5: %i \t C6: %i \t C7: %i \t C8: %i\r\n", controls.channel_1, controls.channel_2, controls.channel_3, controls.channel_4, controls.channel_5, controls.channel_6, controls.channel_7, controls.channel_8);
+
+ pc.printf("Heading Lock: %s \r\n", heading_lock_enabled ? "true" : "false");
+ pc.printf("Heading Lock Speed: %i \r\n", heading_lock_speed);
+ #endif
+ }
+}
+
+
/** Calculate Orientation
*
* Calculates orientation using the BNO055 sensor
@@ -141,20 +222,23 @@
/* If there is an error then we maintain the same
* orientation to stop random control flipping */
if(!bno055Healthy()){
+ #if defined (PC_DEBUGGING) && defined (DEBUG_ORIENTATION)
pc.printf("ERROR: BNO055 has an error/status problem!!!\r\n");
+ #endif
}else{
/* Read in the Euler angles */
orientation = getEulerAngles();
- /* We are upside down in range -30 -> -90 */
+ /* We are upside down in range -30 -> -90
+ * the sensor will report -60 when inverted */
if(orientation.roll < -30 && orientation.roll > -90){
inverted = true;
}else{
inverted = false;
}
#if defined (PC_DEBUGGING) && defined (DEBUG_ORIENTATION)
- pc.printf("Roll=%7.2f, Inverted=%s", orientation.roll, inverted ? "true" : "false");
+ pc.printf("Inverted= %s \t (%7.2f) \r\n", inverted ? "true" : "false", orientation.roll);
#endif
}
@@ -164,53 +248,70 @@
/** Calculate controls
*
* Calculates controls based on if the robot is inverted or not
+ * TODO: Add inverted support
*/
void calculate_controls(){
- if(inverted){
+
+ #ifdef OMNI_MIXER
- }else{
+ float x = direction.x_translation -50;
+ float y = direction.y_translation -50;
- float theta = (float)atan2((double)direction.x_translation, (double)direction.y_translation);
- float magnitude = (float)sqrt((double)((direction.x_translation*direction.x_translation)+(direction.y_translation*direction.y_translation)));
+ float theta = (float)atan2((double)x, (double)y);
+ float magnitude = (float)sqrt((double)((x*x)+(y*y)));
- if(magnitude > 75.0f){
+ if(magnitude > 5.0f){
- float vx = magnitude * cos(theta);
- float vy = magnitude * sin(theta);
- const float sqrt3o2 = 1.0*sqrt(3.0)/2;
+ float vx = magnitude * sin(theta);
+ float vy = magnitude * cos(theta);
+ const float sqrt3o2 = 1.0*sqrt(3.0)/2.0;
- float w0 = -vx; // v dot [-1, 0] / 25mm
- float w1 = 0.5*vx - sqrt3o2 * vy; // v dot [1/2, -sqrt(3)/2] / 25mm
- float w2 = 0.5*vx + sqrt3o2 * vy; // v dot [1/2, +sqrt(3)/2] / 25mm
-
- float w0_speed = map(w0, 0, 600, 0, 100);
- float w1_speed = map(w1, 0, 600, 0, 100);
- float w2_speed = map(w2, 0, 600, 0, 100);
-
- /* Add in rotation */
-
- w0_speed += direction.rotation;
- w1_speed += direction.rotation;
- w2_speed += direction.rotation;
-
- /* Clamp outputs to correct range */
- outputs.wheel_1 = clamp(w0_speed, 0, 100);
- outputs.wheel_2 = clamp(w1_speed, 0, 100);
- outputs.wheel_3 = clamp(w2_speed, 0, 100);
-
-
-
+ float w0 = -vx; // v dot [-1, 0] / 25mm
+ float w1 = 0.5*vx - sqrt3o2 * vy; // v dot [1/2, -sqrt(3)/2] / 25mm
+ float w2 = 0.5*vx + sqrt3o2 * vy; // v dot [1/2, +sqrt(3)/2] / 25mm
+ #if defined (PC_DEBUGGING) && defined (DEBUG_CONTROLS)
+ pc.printf("Calculated Controls: (%7.2f) \t (%7.2f) \t (%7.2f) \r\n", w0, w1, w2);
+ #endif
+ float w0_speed = map(w0, -70, 70, 0, 100);
+ float w1_speed = map(w1, -70, 70, 0, 100);
+ float w2_speed = map(w2, -70, 70, 0, 100);
+
+ /* Add in rotation */
+ #if defined (PC_DEBUGGING) && defined (DEBUG_CONTROLS)
+ pc.printf("Mapped Controls: (%7.2f) \t (%7.2f) \t (%7.2f) \r\n", w0_speed, w1_speed, w2_speed);
+ #endif
+ outputs.wheel_1 += w0_speed -50;
+ outputs.wheel_2 += w1_speed -50;
+ outputs.wheel_3 += w2_speed -50;
+
+
+ }else{
+ outputs.wheel_1 = 50;
+ outputs.wheel_2 = 50;
+ outputs.wheel_3 = 50;
+ }
- }else{
- outputs.wheel_1 = 50;
- outputs.wheel_2 = 50;
- outputs.wheel_3 = 50;
- }
-
-
-
-
- }
+ outputs.wheel_1 += direction.rotation -50;
+ outputs.wheel_2 += direction.rotation -50;
+ outputs.wheel_3 += direction.rotation -50;
+
+ #else
+
+ outputs.wheel_1 = direction.rotation;
+ outputs.wheel_2 = direction.x_translation;
+ outputs.wheel_3 = direction.y_translation;
+
+ #endif
+
+ /* Clamp outputs to correct range */
+ outputs.wheel_1 = clamp(outputs.wheel_1, 0, 100);
+ outputs.wheel_2 = clamp(outputs.wheel_2, 0, 100);
+ outputs.wheel_3 = clamp(outputs.wheel_3, 0, 100);
+
+ #if defined (PC_DEBUGGING) && defined (DEBUG_CONTROLS)
+ pc.printf("Final Controls: %i \t %i \t %i \r\n", outputs.wheel_1, outputs.wheel_2, outputs.wheel_3);
+ #endif
+
}
/** Heading correction
@@ -220,45 +321,83 @@
*
* @param speed speed to rotate in percent
*/
-void calculate_heading_correction(float speed, float deadband){
-
- /* Sanitize speed */
- if(speed > 100)
- speed = 100;
- else if(speed < 0)
- speed = 0;
+void calculate_heading_correction(){
+ if(heading_lock_enabled){
+
+ /* Normalize headings */
+ float n_current_heading = normalize(orientation.heading);
+ float n_heading_lock = normalize(heading_lock);
+
+ #if defined (PC_DEBUGGING) && defined (DEBUG_HEADING_LOCK)
+ pc.printf("Current heading: %f \t Desired Heading: %f \r\n", n_current_heading, n_heading_lock);
+ #endif
+
+ /* Calculate error */
+ float error = n_current_heading - n_heading_lock;
+
+ /* Normalize error */
+ error = normalize(error);
- /* speed is in percent, but controls normal position is at 50% for no movement */
- speed /= 2;
-
- /* deadband specifies total angle to ignore, so split for each side */
- deadband = (deadband / 2);
-
- /* Normalize headings */
- float n_current_heading = normalize(orientation.heading);
- float n_heading_lock = normalize(heading_lock);
+ /* Figure out which way to turn */
+ if(abs(error) > heading_lock_deadband){
+
+ /* Speed is 0 -> 100
+ * error is -180 -> 180
+ * output is 0 -> 100
+ */
+ //error = error * (heading_lock_speed / 100.0);
+ #if defined (PC_DEBUGGING) && defined (DEBUG_HEADING_LOCK)
+ pc.printf("ERROR: %7.2f", error);
+ #endif
+ error = map(error, -180, 180, 1.0, -1.0);
+
+
+ float amount = error * 50;
+
+ amount *= heading_lock_speed / 100.0;
+
+ direction.rotation += amount;
+
+ }else{
+
+ /* In deadband so do nothing */
+ direction.rotation = 50;
+ }
+ #if defined (PC_DEBUGGING) && defined (DEBUG_HEADING_LOCK)
+ pc.printf("Heading error: (%7.2f) \t Rotation: (%7.2f) \t Speed: (%7.2f) \r\n", error, direction.rotation, heading_lock_speed);
+ #endif
+
+ }
+}
+
+/** Output to OmniMixer and ESCs
+ *
+ * Sends calculated output values to devices
+ */
+void output_stage(){
+ if(!failsafe){
+ //outputs.wheel_1 -= 18;
+ //outputs.wheel_2 -= 27;
+ //outputs.wheel_3 -= 14;
- pc.printf("Current heading: %f, Desired Heading: %f", n_current_heading, n_heading_lock);
-
- /* Calculate error */
- float error = n_current_heading - n_heading_lock;
-
- /* Normalize error */
- error = normalize(error);
-
- /* Figure out which way to turn */
- if(error > deadband){
-
- /* Turning left */
- direction.rotation += speed;
- }else if(error < -deadband){
-
- /* Turning right */
- direction.rotation -= speed;
+ esc_omni_1.setThrottle(outputs.wheel_1);
+ esc_omni_2.setThrottle(outputs.wheel_2);
+ esc_omni_3.setThrottle(outputs.wheel_3);
+ //esc_weapon_1.setThrottle(outputs.weapon_motor_1);
+ //esc_weapon_2.setThrottle(outputs.weapon_motor_2);
+ }else{
+ esc_omni_1.failsafe();
+ esc_omni_2.failsafe();
+ esc_omni_3.failsafe();
}
+ #if defined (PC_DEBUGGING) && defined (DEBUG_OUTPUT)
+ pc.printf("OUTPUT M1: %i \t M2: %i \t M3: %i \t W1: %i \t W2: %i \r\n", outputs.wheel_1, outputs.wheel_2, outputs.wheel_3, outputs.weapon_motor_1, outputs.weapon_motor_2);
+ #endif
}
+/******************************************************************************/
+
/** Main
*
* Main Loop
@@ -272,21 +411,31 @@
init();
while(true){
- read_inputs();
-
- //calculate_orientation();
- //calculate_controls();
+ led1 = armed;
+ reset_outputs();
- #ifdef HEADING_LOCK
- calculate_heading_correction(speed, deadband){
+ #ifdef PC_DEBUGGING
+ pc.printf("\x1b[2J\x1b[H");
#endif
- //output_stage();
- wait_ms(20);
- }
-
-
-
+ read_inputs();
+ if(armed && !just_armed){
+ led1 = armed;
+ #ifdef ORIENTATION
+ calculate_orientation();
+ #endif
+
+ #ifdef HEADING_LOCK
+ calculate_heading_correction();
+ #endif
+ calculate_controls();
+
+ output_stage();
+ }
+ #ifdef PC_DEBUGGING
+ wait_ms(50);
+ #endif
+ }
}