Diff: quadCommand/quadCommand.h

Revision:

0:853ffcef6c67

Child:

1:e4439be6e1b9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/quadCommand/quadCommand.h	Wed Oct 15 04:59:58 2014 +0000
@@ -0,0 +1,122 @@
+/************************* quadCommand.h *********************************/
+/*                                                                       */
+/*************************************************************************/
+
+#ifndef QUAD_COMMAND_H
+#define QUAD_COMMAND_H
+
+#include "mbed.h"
+#include "motor.h"
+#include "com.h"
+#include "Sensor.h"
+#include "PID.h"
+#include "KL25Z_Watchdog.h"
+#include "RGBled.h"
+
+
+#define DOGTIMER 0.5f
+
+// Motor constants.
+#define MOTOR1              PTA5    // Pin used for motor 1.
+#define MOTOR2              PTA4    // Pin used for motor 2. 
+#define MOTOR3              PTA12   // Pin used for motor 3.
+#define MOTOR4              PTD4    // Pin used for motor 4. 
+
+// Xbee constants.
+#define TXPIN               PTA2        // Pin used for xbee TX.
+#define RXPIN               PTA1        // Pin used for xbee RX.
+#define RSSIPIN             PTD5        // Pin used for xbee RSSI
+
+
+// Sensor constants.
+#define MMA8451_I2C_ADDRESS (0x1d<<1)   // Address of I2C accelerometer.
+#define ACCSDA              PTE25       // Pin for accelerometer SDA line.
+#define ACCSCL              PTE24       // Pin for accelerometer SCL line.
+
+#define ROLLTHROTTLEGAIN    0
+#define PITCHTHROTTLEGAIN   0
+#define YAWTHROTTLEGAIN     4
+
+// PID constants. 
+// D = 450 shaking
+//High throttle(<200) 0.6     0   450
+//Low throttle(<100)  0.45    0.1 80
+#define PID_P_PITCH 1.1                     // Proportional gain.
+#define PID_I_PITCH 0.0                       // Integral gain.
+#define PID_D_PITCH 140.0                       // Derivative gain.
+#define PID_P_ROLL 1.1                       // Proportional gain.
+#define PID_I_ROLL 0.0                     // Integral gain.
+#define PID_D_ROLL 140.0                       // Derivative gain.
+#define PID_P_YAW 2.2                    // Proportional gain.
+#define PID_I_YAW 0.0                       // Integral gain.
+#define PID_D_YAW 40.0                       // Derivative gain.
+#define TRIM_VALUE 0.5f
+
+#define THROTTLE_THRES 16
+
+#define PITCH_RANGE 10
+#define PITCH_MULTI 4
+#define PITCH_LOW_RANGE -PITCH_RANGE*PITCH_MULTI+64
+#define PITCH_HI_RANGE PITCH_RANGE*PITCH_MULTI+64
+
+#define YAW_RANGE 60
+#define YAW_MULTI 1
+#define YAW_LOW_RANGE -YAW_RANGE*YAW_MULTI+64
+#define YAW_HI_RANGE YAW_RANGE*YAW_MULTI+64
+
+#define MAX_CORRECTION_VALUE 10.0       // Integral windup.
+#define DEFAULT_WINDUP_GUARD 10         //MAX_CORRECTION_VALUE / PID_I_PITCH       // Integral windup.
+#define YAW_WINDUP_GUARD 50
+
+#define MOTOR_UPDATE    10             // Motor update speed.
+#define TIMEOUT_TIMER   2.0
+
+#define THROTTLEPERSEC  500.0 
+#define THROTTLEJUMP THROTTLEPERSEC / 1000.0 * MOTOR_UPDATE
+#define TIMEOUT  TIMEOUT_TIMER / MOTOR_UPDATE * 1000
+
+#define ROLLYAWJUMP 2.0
+#define YAWCUTOFF 10
+
+class quadCommand
+{
+    public:
+    quadCommand();  // Constructor
+    void run();     // Main loop
+    
+    private:
+    void rxInput();     // Receive data from Xbee
+    void updateMotors();    // Send PID values to Motors
+    void readyMotors();     // Callback function for update
+    
+    com *myCom;
+    Sensor *mpu;
+    RGBled led;
+    motor myMotor1;
+    motor myMotor2;
+    motor myMotor3;
+    motor myMotor4;
+    PID pidPitch;
+    PID pidRoll;
+    PID pidYaw;
+    Timer time;
+    Watchdog dog;
+    Ticker motorProcess;
+    
+    volatile bool need2update;
+         
+    //Control Variables
+    float targetThrottle,   previousThrottle,   throttleHelper,     throttle;
+    float targetPitch,      followPitch,        currentPitch,       pitch;
+    float targetRoll,       followRoll,         currentRoll,        roll;
+//    float targetYaw,        followYaw,        currentYaw,         yaw;
+    float targetGyroYaw,    followGyroYaw,    currentGyroYaw,     gyroYaw;
+    
+    float pitchTrim, rollTrim , gyroYawTrim;
+    
+    unsigned int rxTimeout;
+    unsigned int dt;
+    
+};
+
+#endif
\ No newline at end of file