Diff: main.cpp

Revision:

0:d6eeeae3c3cb

Child:

1:026f79cfd378

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Nov 18 14:22:26 2016 +0000
@@ -0,0 +1,292 @@
+/* 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)
+
+/* 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);
+
+/* 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
+
+/* LEDs */
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+
+/* Initial control positions */
+static struct rc_controls controls = {
+    .channel_1 = 50,
+    .channel_2 = 50,
+    .channel_3 = 50,
+    .channel_4 = 0,
+    .channel_5 = 0,
+    .channel_6 = 0,
+    .channel_7 = 0,
+    .channel_8 = 0
+};
+
+/* Initial output positions */
+static struct rc_outputs outputs = {
+    .wheel_1 = 50,
+    .wheel_2 = 50,
+    .wheel_3 = 50,
+    .weapon_motor_1 = 0,
+    .weapon_motor_2 = 0
+};
+
+/* Direction vector */
+static struct direction_vector direction = {
+    .rotation = 50,
+    .x_translation = 50,
+    .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
+ * 
+ * Sets up devices
+ * Will loop init for bno055 untill it starts correctly.
+ */
+void init(){
+    #ifdef ORIENTATION
+    init_bno055:
+    /* Initialise the sensor */
+    if(!initBNO055()){
+        
+        /* There was a problem detecting the BNO055 ... check your connections */
+        #if defined (PC_DEBUGGING) && defined (DEBUG_ORIENTATION)  
+        pc.printf("BNO055 not detected\r\n");
+        #endif        
+        goto init_bno055;
+        
+    } else {   
+        #if defined (PC_DEBUGGING) && defined (DEBUG_ORIENTATION)   
+        pc.printf("BNO055 was detected!\r\n");
+        #endif
+    }
+    #endif
+}
+
+/** Calculate Orientation
+ * 
+ * Calculates orientation using the BNO055 sensor
+ */
+void calculate_orientation(){    
+    
+    /* If there is an error then we maintain the same 
+     * orientation to stop random control flipping */
+    if(!bno055Healthy()){
+        pc.printf("ERROR: BNO055 has an error/status problem!!!\r\n");
+    }else{       
+        
+        /* Read in the Euler angles */
+        orientation = getEulerAngles();    
+        
+        /* We are upside down in range -30 -> -90 */
+        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");
+        #endif
+    }        
+    
+    
+}
+
+/** Calculate controls
+ * 
+ * Calculates controls based on if the robot is inverted or not
+ */
+void calculate_controls(){
+    if(inverted){
+    
+    }else{  
+    
+        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)));
+    
+        if(magnitude > 75.0f){  
+    
+            float vx = magnitude * cos(theta);
+            float vy = magnitude * sin(theta);
+            const float sqrt3o2 = 1.0*sqrt(3.0)/2;
+    
+            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);
+            
+            
+            
+    
+        }else{
+            outputs.wheel_1 = 50;
+            outputs.wheel_2 = 50;
+            outputs.wheel_3 = 50;
+        }
+             
+        
+        
+        
+    }
+}
+
+/** Heading correction
+ *
+ *  Works out which way to spin to return to set heading
+ *  Based on tiberius turnto code
+ *
+ *  @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;
+        
+    /* 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);    
+    
+    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;        
+    }
+    
+}
+
+/** Main
+ * 
+ * Main Loop
+ */
+int main() {
+    #ifdef PC_DEBUGGING
+    pc.baud(115200);
+    pc.printf("Triforce Control System \r\n");
+    #endif
+    
+    init();
+    
+    while(true){
+        read_inputs();      
+          
+        //calculate_orientation();
+        //calculate_controls();
+        
+        #ifdef HEADING_LOCK
+        calculate_heading_correction(speed, deadband){
+        #endif
+        //output_stage();
+        
+        wait_ms(20);
+    }
+    
+    
+    
+}
+
+