Prosper Van / Mbed 2 deprecated quadCommand7v2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed PID Sensorv2 xbeeCom

quadCommand/quadCommand.h@1:e4439be6e1b9, 2014-11-02 (annotated)

Committer:
oprospero
Date:
Sun Nov 02 19:18:55 2014 +0000
Revision:
1:e4439be6e1b9
Parent:
0:853ffcef6c67 
motor2 not working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
oprospero 0:853ffcef6c67 1 /************************* quadCommand.h *********************************/
oprospero 0:853ffcef6c67 2 /* */
oprospero 0:853ffcef6c67 3 /*************************************************************************/
oprospero 0:853ffcef6c67 4
oprospero 0:853ffcef6c67 5 #ifndef QUAD_COMMAND_H
oprospero 0:853ffcef6c67 6 #define QUAD_COMMAND_H
oprospero 0:853ffcef6c67 7
oprospero 0:853ffcef6c67 8 #include "mbed.h"
oprospero 0:853ffcef6c67 9 #include "motor.h"
oprospero 0:853ffcef6c67 10 #include "com.h"
oprospero 0:853ffcef6c67 11 #include "Sensor.h"
oprospero 0:853ffcef6c67 12 #include "PID.h"
oprospero 0:853ffcef6c67 13 #include "KL25Z_Watchdog.h"
oprospero 0:853ffcef6c67 14 #include "RGBled.h"
oprospero 0:853ffcef6c67 15
oprospero 0:853ffcef6c67 16
oprospero 0:853ffcef6c67 17 #define DOGTIMER 0.5f
oprospero 0:853ffcef6c67 18
oprospero 0:853ffcef6c67 19 // Motor constants.
oprospero 1:e4439be6e1b9 20 #define MOTOR1 PTA5 // PTA5 // Pin used for motor 1.
oprospero 1:e4439be6e1b9 21 #define MOTOR2 PTA4 // PTA4 // Pin used for motor 2.
oprospero 1:e4439be6e1b9 22 #define MOTOR3 PTA12 // PTA12 // Pin used for motor 3.
oprospero 1:e4439be6e1b9 23 #define MOTOR4 PTD4 // PTD4 // Pin used for motor 4.
oprospero 0:853ffcef6c67 24
oprospero 0:853ffcef6c67 25 // Xbee constants.
oprospero 0:853ffcef6c67 26 #define TXPIN PTA2 // Pin used for xbee TX.
oprospero 0:853ffcef6c67 27 #define RXPIN PTA1 // Pin used for xbee RX.
oprospero 0:853ffcef6c67 28 #define RSSIPIN PTD5 // Pin used for xbee RSSI
oprospero 0:853ffcef6c67 29
oprospero 0:853ffcef6c67 30
oprospero 0:853ffcef6c67 31 // Sensor constants.
oprospero 0:853ffcef6c67 32 #define MMA8451_I2C_ADDRESS (0x1d<<1) // Address of I2C accelerometer.
oprospero 0:853ffcef6c67 33 #define ACCSDA PTE25 // Pin for accelerometer SDA line.
oprospero 0:853ffcef6c67 34 #define ACCSCL PTE24 // Pin for accelerometer SCL line.
oprospero 0:853ffcef6c67 35
oprospero 0:853ffcef6c67 36 #define ROLLTHROTTLEGAIN 0
oprospero 0:853ffcef6c67 37 #define PITCHTHROTTLEGAIN 0
oprospero 0:853ffcef6c67 38 #define YAWTHROTTLEGAIN 4
oprospero 0:853ffcef6c67 39
oprospero 0:853ffcef6c67 40 // PID constants.
oprospero 0:853ffcef6c67 41 // D = 450 shaking
oprospero 0:853ffcef6c67 42 //High throttle(<200) 0.6 0 450
oprospero 0:853ffcef6c67 43 //Low throttle(<100) 0.45 0.1 80
oprospero 0:853ffcef6c67 44 #define PID_P_PITCH 1.1 // Proportional gain.
oprospero 0:853ffcef6c67 45 #define PID_I_PITCH 0.0 // Integral gain.
oprospero 0:853ffcef6c67 46 #define PID_D_PITCH 140.0 // Derivative gain.
oprospero 0:853ffcef6c67 47 #define PID_P_ROLL 1.1 // Proportional gain.
oprospero 0:853ffcef6c67 48 #define PID_I_ROLL 0.0 // Integral gain.
oprospero 0:853ffcef6c67 49 #define PID_D_ROLL 140.0 // Derivative gain.
oprospero 0:853ffcef6c67 50 #define PID_P_YAW 2.2 // Proportional gain.
oprospero 0:853ffcef6c67 51 #define PID_I_YAW 0.0 // Integral gain.
oprospero 0:853ffcef6c67 52 #define PID_D_YAW 40.0 // Derivative gain.
oprospero 0:853ffcef6c67 53 #define TRIM_VALUE 0.5f
oprospero 0:853ffcef6c67 54
oprospero 0:853ffcef6c67 55 #define THROTTLE_THRES 16
oprospero 0:853ffcef6c67 56
oprospero 0:853ffcef6c67 57 #define PITCH_RANGE 10
oprospero 0:853ffcef6c67 58 #define PITCH_MULTI 4
oprospero 0:853ffcef6c67 59 #define PITCH_LOW_RANGE -PITCH_RANGE*PITCH_MULTI+64
oprospero 0:853ffcef6c67 60 #define PITCH_HI_RANGE PITCH_RANGE*PITCH_MULTI+64
oprospero 0:853ffcef6c67 61
oprospero 0:853ffcef6c67 62 #define YAW_RANGE 60
oprospero 0:853ffcef6c67 63 #define YAW_MULTI 1
oprospero 0:853ffcef6c67 64 #define YAW_LOW_RANGE -YAW_RANGE*YAW_MULTI+64
oprospero 0:853ffcef6c67 65 #define YAW_HI_RANGE YAW_RANGE*YAW_MULTI+64
oprospero 0:853ffcef6c67 66
oprospero 0:853ffcef6c67 67 #define MAX_CORRECTION_VALUE 10.0 // Integral windup.
oprospero 0:853ffcef6c67 68 #define DEFAULT_WINDUP_GUARD 10 //MAX_CORRECTION_VALUE / PID_I_PITCH // Integral windup.
oprospero 0:853ffcef6c67 69 #define YAW_WINDUP_GUARD 50
oprospero 0:853ffcef6c67 70
oprospero 0:853ffcef6c67 71 #define MOTOR_UPDATE 10 // Motor update speed.
oprospero 0:853ffcef6c67 72 #define TIMEOUT_TIMER 2.0
oprospero 0:853ffcef6c67 73
oprospero 0:853ffcef6c67 74 #define THROTTLEPERSEC 500.0
oprospero 0:853ffcef6c67 75 #define THROTTLEJUMP THROTTLEPERSEC / 1000.0 * MOTOR_UPDATE
oprospero 0:853ffcef6c67 76 #define TIMEOUT TIMEOUT_TIMER / MOTOR_UPDATE * 1000
oprospero 0:853ffcef6c67 77
oprospero 0:853ffcef6c67 78 #define ROLLYAWJUMP 2.0
oprospero 0:853ffcef6c67 79 #define YAWCUTOFF 10
oprospero 0:853ffcef6c67 80
oprospero 0:853ffcef6c67 81 class quadCommand
oprospero 0:853ffcef6c67 82 {
oprospero 0:853ffcef6c67 83 public:
oprospero 0:853ffcef6c67 84 quadCommand(); // Constructor
oprospero 0:853ffcef6c67 85 void run(); // Main loop
oprospero 0:853ffcef6c67 86
oprospero 0:853ffcef6c67 87 private:
oprospero 0:853ffcef6c67 88 void rxInput(); // Receive data from Xbee
oprospero 0:853ffcef6c67 89 void updateMotors(); // Send PID values to Motors
oprospero 0:853ffcef6c67 90 void readyMotors(); // Callback function for update
oprospero 0:853ffcef6c67 91
oprospero 1:e4439be6e1b9 92 com *myCom; // Serial
oprospero 1:e4439be6e1b9 93 Sensor *mpu; // I2C
oprospero 1:e4439be6e1b9 94 motor *myMotor1; // 1 PWM
oprospero 1:e4439be6e1b9 95 motor *myMotor2; // 1 PWM
oprospero 1:e4439be6e1b9 96 motor *myMotor3; // 1 PWM
oprospero 1:e4439be6e1b9 97 motor *myMotor4; // 1 PWM
oprospero 1:e4439be6e1b9 98 PID pidPitch;
oprospero 0:853ffcef6c67 99 PID pidRoll;
oprospero 0:853ffcef6c67 100 PID pidYaw;
oprospero 1:e4439be6e1b9 101 RGBled led; // 3 PWM
oprospero 1:e4439be6e1b9 102 Timer time;
oprospero 1:e4439be6e1b9 103 Watchdog dog; // Timer
oprospero 0:853ffcef6c67 104 Ticker motorProcess;
oprospero 0:853ffcef6c67 105
oprospero 0:853ffcef6c67 106 volatile bool need2update;
oprospero 0:853ffcef6c67 107
oprospero 0:853ffcef6c67 108 //Control Variables
oprospero 0:853ffcef6c67 109 float targetThrottle, previousThrottle, throttleHelper, throttle;
oprospero 0:853ffcef6c67 110 float targetPitch, followPitch, currentPitch, pitch;
oprospero 0:853ffcef6c67 111 float targetRoll, followRoll, currentRoll, roll;
oprospero 0:853ffcef6c67 112 // float targetYaw, followYaw, currentYaw, yaw;
oprospero 0:853ffcef6c67 113 float targetGyroYaw, followGyroYaw, currentGyroYaw, gyroYaw;
oprospero 0:853ffcef6c67 114
oprospero 0:853ffcef6c67 115 float pitchTrim, rollTrim , gyroYawTrim;
oprospero 0:853ffcef6c67 116
oprospero 0:853ffcef6c67 117 unsigned int rxTimeout;
oprospero 0:853ffcef6c67 118 unsigned int dt;
oprospero 0:853ffcef6c67 119
oprospero 0:853ffcef6c67 120 };
oprospero 0:853ffcef6c67 121
oprospero 0:853ffcef6c67 122 #endif