sdカードに焦点調整を追加
Dependencies: mbed MPU6050_2 HMC5883L_2 SDFileSystem3
main.cpp@11:d44d137831b9, 2019-02-10 (annotated)
- Committer:
- taknokolat
- Date:
- Sun Feb 10 18:13:37 2019 +0000
- Revision:
- 11:d44d137831b9
- Parent:
- 10:63fe920595a7
- Child:
- 12:083662bca47d
a;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
HARUKIDELTA | 0:84ddd6d354e1 | 1 | #include "mbed.h" |
Skykon | 3:c18342e4fddd | 2 | #include "MPU6050_DMP6.h" |
taknokolat | 6:166746820555 | 3 | #include "HMC5883L.h" |
Skykon | 3:c18342e4fddd | 4 | |
Skykon | 3:c18342e4fddd | 5 | //MPU_check用 |
Skykon | 3:c18342e4fddd | 6 | #define PI 3.14159265358979 |
HARUKIDELTA | 0:84ddd6d354e1 | 7 | |
taknokolat | 11:d44d137831b9 | 8 | #define servo_NEUTRAL_R 1614 |
taknokolat | 11:d44d137831b9 | 9 | #define servo_NEUTRAL_L 1621 |
HARUKIDELTA | 0:84ddd6d354e1 | 10 | #define servo_FORWARD_R 1860 |
HARUKIDELTA | 0:84ddd6d354e1 | 11 | #define servo_FORWARD_L 1860 |
HARUKIDELTA | 0:84ddd6d354e1 | 12 | #define servo_back_R 1060 |
HARUKIDELTA | 0:84ddd6d354e1 | 13 | #define servo_back_L 1060 |
HARUKIDELTA | 0:84ddd6d354e1 | 14 | #define servo_slow_FORWARD_R 1560 |
HARUKIDELTA | 0:84ddd6d354e1 | 15 | #define servo_slow_FORWARD_L 1560 |
HARUKIDELTA | 0:84ddd6d354e1 | 16 | #define servo_slow_back_R 1360 |
HARUKIDELTA | 0:84ddd6d354e1 | 17 | #define servo_slow_back_L 1360 |
taknokolat | 11:d44d137831b9 | 18 | #define TurnTable_NEUTRAL 1180 //カメラ台座のサーボ |
taknokolat | 7:8989a4b84695 | 19 | #define MatchSpeed 1500 + 100 //カメラと方向を合わせるときの車輪の速度 |
taknokolat | 11:d44d137831b9 | 20 | #define Focus_NEUTRAL 1455 //焦点合わせ用サーボの最小値 |
HARUKIDELTA | 0:84ddd6d354e1 | 21 | |
HARUKIDELTA | 0:84ddd6d354e1 | 22 | #define MOVE_NEUTRAL 0 |
HARUKIDELTA | 0:84ddd6d354e1 | 23 | #define MOVE_FORWARD 1 |
HARUKIDELTA | 0:84ddd6d354e1 | 24 | #define MOVE_LEFT 2 |
HARUKIDELTA | 0:84ddd6d354e1 | 25 | #define MOVE_RIGHT 3 |
HARUKIDELTA | 0:84ddd6d354e1 | 26 | #define MOVE_BACK 4 |
Skykon | 3:c18342e4fddd | 27 | #define GOAL_FORWARD 5 //ゴール付近_ゆっくり |
HARUKIDELTA | 0:84ddd6d354e1 | 28 | #define GOAL_LEFT 6 |
HARUKIDELTA | 0:84ddd6d354e1 | 29 | #define GOAL_RIGHT 7 |
Skykon | 3:c18342e4fddd | 30 | #define MAX_FORWARD 8 //はやい_姿勢修正用 |
HARUKIDELTA | 0:84ddd6d354e1 | 31 | #define MAX_BACK 9 |
HARUKIDELTA | 0:84ddd6d354e1 | 32 | |
Skykon | 3:c18342e4fddd | 33 | |
taknokolat | 1:290e621741fd | 34 | void getSF_Serial_jevois(); |
taknokolat | 1:290e621741fd | 35 | void getSF_Serial_pi(); |
taknokolat | 1:290e621741fd | 36 | |
Skykon | 3:c18342e4fddd | 37 | //MPU_check用 |
Skykon | 3:c18342e4fddd | 38 | void SensingMPU(); |
taknokolat | 7:8989a4b84695 | 39 | |
taknokolat | 7:8989a4b84695 | 40 | void MoveCansat(char g_landingcommand); |
Skykon | 3:c18342e4fddd | 41 | void setup(); |
Skykon | 3:c18342e4fddd | 42 | void Init_sensors(); |
Skykon | 3:c18342e4fddd | 43 | void DisplayClock(); |
taknokolat | 4:67f705d42f1e | 44 | void DebugPrint(); |
taknokolat | 6:166746820555 | 45 | void SensingHMC(); |
taknokolat | 7:8989a4b84695 | 46 | void MoveCameraBoard(); |
taknokolat | 7:8989a4b84695 | 47 | void MatchPosition(); |
taknokolat | 7:8989a4b84695 | 48 | void FocusAdjust(); |
Skykon | 3:c18342e4fddd | 49 | |
taknokolat | 6:166746820555 | 50 | static float nowAngle[3] = {0,0,0},nowAngle_HMC=0; |
taknokolat | 6:166746820555 | 51 | float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0,g_FirstYAW_HMC; |
Skykon | 3:c18342e4fddd | 52 | |
taknokolat | 7:8989a4b84695 | 53 | bool setupFlag = false; |
taknokolat | 7:8989a4b84695 | 54 | bool CameraDegFlag = false; |
taknokolat | 7:8989a4b84695 | 55 | bool jevoisFlag = true; |
taknokolat | 4:67f705d42f1e | 56 | |
Skykon | 3:c18342e4fddd | 57 | enum Angle{ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度 |
Skykon | 3:c18342e4fddd | 58 | |
Skykon | 3:c18342e4fddd | 59 | Timer t; |
Skykon | 3:c18342e4fddd | 60 | |
taknokolat | 11:d44d137831b9 | 61 | RawSerial pc(PA_2,PA_3,115200); //uart2 |
taknokolat | 11:d44d137831b9 | 62 | //pa2:UART2_TX,pa3:UART2_RX |
taknokolat | 11:d44d137831b9 | 63 | //RawSerial pc2(PB_6,PB_7,115200); //uart1 |
taknokolat | 11:d44d137831b9 | 64 | //pb6:UART1_TX,pb7:UART1_RX |
taknokolat | 11:d44d137831b9 | 65 | |
HARUKIDELTA | 5:8bfe95431ec0 | 66 | //PWM pin宣言 |
taknokolat | 6:166746820555 | 67 | PwmOut servoR(PC_6); //TIM3_CH1 車輪右 |
taknokolat | 6:166746820555 | 68 | PwmOut servoL(PC_7); //TIM3_CH2 車輪左 |
HARUKIDELTA | 5:8bfe95431ec0 | 69 | PwmOut servoTurnTable(PB_0); //TIM3_CH3 カメラ台回転Servo |
HARUKIDELTA | 5:8bfe95431ec0 | 70 | PwmOut servoCameradeg(PB_1); //TIM3_CH4 カメラ角度調節Servo |
HARUKIDELTA | 5:8bfe95431ec0 | 71 | PwmOut servoCameraPinto(PB_6); //TIM4_CH1 カメラピント合わせ |
HARUKIDELTA | 5:8bfe95431ec0 | 72 | PwmOut servoCameramount(PA_6); //skipperウラ カメラマウント起動 |
HARUKIDELTA | 5:8bfe95431ec0 | 73 | PwmOut servoGetUP(PC_8); //skipperウラ 起き上がり動作 |
HARUKIDELTA | 5:8bfe95431ec0 | 74 | PwmOut servoParachute(PA_11); //skipper USB端子より パラシュート切り離し |
Skykon | 3:c18342e4fddd | 75 | |
HARUKIDELTA | 5:8bfe95431ec0 | 76 | /*通信用のpinは |
HARUKIDELTA | 5:8bfe95431ec0 | 77 | PA_3(UART2_Rx) skipperウラ |
HARUKIDELTA | 5:8bfe95431ec0 | 78 | PA_12(UART6_Rx) skipperオモテ USB端子より |
HARUKIDELTA | 5:8bfe95431ec0 | 79 | PB_7 (UART1_Rx) skipperオモテ 6番目 TIM4_CH2 |
HARUKIDELTA | 5:8bfe95431ec0 | 80 | */ |
HARUKIDELTA | 5:8bfe95431ec0 | 81 | |
HARUKIDELTA | 5:8bfe95431ec0 | 82 | /*超音波はRaspberryPiに積む*/ |
HARUKIDELTA | 5:8bfe95431ec0 | 83 | |
HARUKIDELTA | 5:8bfe95431ec0 | 84 | //外付けコンパス |
taknokolat | 6:166746820555 | 85 | HMC5883L compass(PB_9, PB_8); //コンパスセンサー TIM4_CH3とCH4 |
HARUKIDELTA | 0:84ddd6d354e1 | 86 | |
taknokolat | 2:f30666d7838b | 87 | char g_landingcommand='N'; |
taknokolat | 1:290e621741fd | 88 | |
Skykon | 3:c18342e4fddd | 89 | //MPU_check用 |
Skykon | 3:c18342e4fddd | 90 | MPU6050DMP6 mpu6050(PC_0,&pc); |
Skykon | 3:c18342e4fddd | 91 | |
Skykon | 3:c18342e4fddd | 92 | |
HARUKIDELTA | 0:84ddd6d354e1 | 93 | int main() { |
taknokolat | 1:290e621741fd | 94 | |
Skykon | 3:c18342e4fddd | 95 | //MPU_check |
Skykon | 3:c18342e4fddd | 96 | setup(); |
Skykon | 3:c18342e4fddd | 97 | |
taknokolat | 11:d44d137831b9 | 98 | servoCameradeg.pulsewidth_us(1400); |
taknokolat | 11:d44d137831b9 | 99 | |
taknokolat | 1:290e621741fd | 100 | // シリアル通信受信の割り込みイベント登録 |
taknokolat | 1:290e621741fd | 101 | pc.attach(getSF_Serial_jevois, Serial::RxIrq); |
taknokolat | 11:d44d137831b9 | 102 | //pc2.attach(getSF_Serial_pi, Serial::RxIrq); |
taknokolat | 1:290e621741fd | 103 | |
HARUKIDELTA | 0:84ddd6d354e1 | 104 | while(1) { |
taknokolat | 7:8989a4b84695 | 105 | if(jevoisFlag==true) NVIC_DisableIRQ(USART1_IRQn); |
taknokolat | 7:8989a4b84695 | 106 | else NVIC_DisableIRQ(USART2_IRQn); |
taknokolat | 8:d11a59d2a2f1 | 107 | |
taknokolat | 10:63fe920595a7 | 108 | MoveCameraBoard(); |
taknokolat | 10:63fe920595a7 | 109 | |
taknokolat | 7:8989a4b84695 | 110 | if(g_landingcommand!='N') MatchPosition(); |
taknokolat | 8:d11a59d2a2f1 | 111 | |
taknokolat | 8:d11a59d2a2f1 | 112 | if(jevoisFlag==true) NVIC_DisableIRQ(USART1_IRQn); |
taknokolat | 8:d11a59d2a2f1 | 113 | else NVIC_DisableIRQ(USART2_IRQn); |
taknokolat | 8:d11a59d2a2f1 | 114 | |
taknokolat | 7:8989a4b84695 | 115 | MoveCansat(g_landingcommand); |
Skykon | 3:c18342e4fddd | 116 | wait_ms(23); |
taknokolat | 8:d11a59d2a2f1 | 117 | |
taknokolat | 7:8989a4b84695 | 118 | if(jevoisFlag==true) NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 7:8989a4b84695 | 119 | else NVIC_EnableIRQ(USART2_IRQn); |
HARUKIDELTA | 0:84ddd6d354e1 | 120 | } |
HARUKIDELTA | 0:84ddd6d354e1 | 121 | } |
HARUKIDELTA | 0:84ddd6d354e1 | 122 | |
Skykon | 3:c18342e4fddd | 123 | |
taknokolat | 1:290e621741fd | 124 | void MoveCansat(char g_landingcommand) |
HARUKIDELTA | 0:84ddd6d354e1 | 125 | { |
taknokolat | 2:f30666d7838b | 126 | //NVIC_DisableIRQ(USART1_IRQn); |
taknokolat | 7:8989a4b84695 | 127 | //NVIC_DisableIRQ(USART2_IRQn); |
taknokolat | 1:290e621741fd | 128 | switch(g_landingcommand){ |
taknokolat | 2:f30666d7838b | 129 | case 'N': //MOVE_NEUTRAL |
HARUKIDELTA | 0:84ddd6d354e1 | 130 | servoR.pulsewidth_us(servo_NEUTRAL_R); |
HARUKIDELTA | 0:84ddd6d354e1 | 131 | servoL.pulsewidth_us(servo_NEUTRAL_L); |
taknokolat | 2:f30666d7838b | 132 | //NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 7:8989a4b84695 | 133 | //NVIC_EnableIRQ(USART2_IRQn); |
HARUKIDELTA | 0:84ddd6d354e1 | 134 | break; |
taknokolat | 1:290e621741fd | 135 | |
taknokolat | 2:f30666d7838b | 136 | case 'Y': //MOVE_FORWARD |
HARUKIDELTA | 0:84ddd6d354e1 | 137 | servoR.pulsewidth_us(servo_FORWARD_R); |
HARUKIDELTA | 0:84ddd6d354e1 | 138 | servoL.pulsewidth_us(servo_FORWARD_L); |
taknokolat | 2:f30666d7838b | 139 | //NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 7:8989a4b84695 | 140 | //NVIC_EnableIRQ(USART2_IRQn); |
HARUKIDELTA | 0:84ddd6d354e1 | 141 | break; |
taknokolat | 1:290e621741fd | 142 | |
taknokolat | 1:290e621741fd | 143 | case 'L': //MOVE_LEFT |
HARUKIDELTA | 0:84ddd6d354e1 | 144 | servoR.pulsewidth_us(servo_slow_FORWARD_R); |
HARUKIDELTA | 0:84ddd6d354e1 | 145 | servoL.pulsewidth_us(servo_slow_back_L); |
taknokolat | 2:f30666d7838b | 146 | //NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 7:8989a4b84695 | 147 | //NVIC_EnableIRQ(USART2_IRQn); |
taknokolat | 1:290e621741fd | 148 | |
taknokolat | 2:f30666d7838b | 149 | case 'R': //MOVE_RIGHT |
HARUKIDELTA | 0:84ddd6d354e1 | 150 | servoR.pulsewidth_us(servo_slow_back_R); |
HARUKIDELTA | 0:84ddd6d354e1 | 151 | servoL.pulsewidth_us(servo_slow_FORWARD_L); |
taknokolat | 2:f30666d7838b | 152 | //NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 7:8989a4b84695 | 153 | //NVIC_EnableIRQ(USART2_IRQn); |
HARUKIDELTA | 0:84ddd6d354e1 | 154 | break; |
taknokolat | 1:290e621741fd | 155 | |
taknokolat | 1:290e621741fd | 156 | case 'B': //MOVE_BACK |
HARUKIDELTA | 0:84ddd6d354e1 | 157 | servoR.pulsewidth_us(servo_back_R); |
HARUKIDELTA | 0:84ddd6d354e1 | 158 | servoL.pulsewidth_us(servo_back_L); |
taknokolat | 2:f30666d7838b | 159 | //NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 7:8989a4b84695 | 160 | //NVIC_EnableIRQ(USART2_IRQn); |
HARUKIDELTA | 0:84ddd6d354e1 | 161 | break; |
taknokolat | 1:290e621741fd | 162 | |
taknokolat | 1:290e621741fd | 163 | case 'G': //GOAL_FORWARD |
HARUKIDELTA | 0:84ddd6d354e1 | 164 | servoR.pulsewidth_us(servo_slow_FORWARD_R); |
HARUKIDELTA | 0:84ddd6d354e1 | 165 | servoL.pulsewidth_us(servo_slow_FORWARD_L); |
taknokolat | 2:f30666d7838b | 166 | //NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 7:8989a4b84695 | 167 | //NVIC_EnableIRQ(USART2_IRQn); |
taknokolat | 7:8989a4b84695 | 168 | break; |
taknokolat | 7:8989a4b84695 | 169 | |
taknokolat | 7:8989a4b84695 | 170 | case 'J': //MOVE_FORWARD Tim |
taknokolat | 7:8989a4b84695 | 171 | servoR.pulsewidth_us(servo_FORWARD_R); |
taknokolat | 7:8989a4b84695 | 172 | servoL.pulsewidth_us(servo_FORWARD_L); |
taknokolat | 7:8989a4b84695 | 173 | //NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 7:8989a4b84695 | 174 | wait(5); |
taknokolat | 10:63fe920595a7 | 175 | do{ |
taknokolat | 10:63fe920595a7 | 176 | }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20)); |
taknokolat | 7:8989a4b84695 | 177 | //NVIC_EnableIRQ(USART2_IRQn); |
taknokolat | 7:8989a4b84695 | 178 | break; |
taknokolat | 7:8989a4b84695 | 179 | |
taknokolat | 7:8989a4b84695 | 180 | case 'M': //MatchPosition |
taknokolat | 7:8989a4b84695 | 181 | servoR.pulsewidth_us(MatchSpeed); |
taknokolat | 7:8989a4b84695 | 182 | //NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 7:8989a4b84695 | 183 | //NVIC_EnableIRQ(USART2_IRQn); |
taknokolat | 7:8989a4b84695 | 184 | break; |
taknokolat | 7:8989a4b84695 | 185 | |
taknokolat | 7:8989a4b84695 | 186 | case 'T': //jevoisからraspberry piへの切り替え |
taknokolat | 7:8989a4b84695 | 187 | jevoisFlag = false; |
taknokolat | 7:8989a4b84695 | 188 | //NVIC_EnableIRQ(USART2_IRQn); |
HARUKIDELTA | 0:84ddd6d354e1 | 189 | break; |
taknokolat | 1:290e621741fd | 190 | |
taknokolat | 1:290e621741fd | 191 | default : |
taknokolat | 2:f30666d7838b | 192 | //NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 7:8989a4b84695 | 193 | //NVIC_EnableIRQ(USART2_IRQn); |
HARUKIDELTA | 0:84ddd6d354e1 | 194 | break; |
taknokolat | 1:290e621741fd | 195 | |
HARUKIDELTA | 0:84ddd6d354e1 | 196 | } |
taknokolat | 7:8989a4b84695 | 197 | |
HARUKIDELTA | 0:84ddd6d354e1 | 198 | return; |
taknokolat | 1:290e621741fd | 199 | } |
taknokolat | 1:290e621741fd | 200 | |
taknokolat | 7:8989a4b84695 | 201 | void MoveCameraBoard(){ |
taknokolat | 7:8989a4b84695 | 202 | MoveCansat('N'); |
taknokolat | 10:63fe920595a7 | 203 | g_landingcommand='N'; |
taknokolat | 11:d44d137831b9 | 204 | servoTurnTable.pulsewidth_us(1800); |
taknokolat | 11:d44d137831b9 | 205 | wait_ms(600); |
taknokolat | 7:8989a4b84695 | 206 | servoTurnTable.pulsewidth_us(TurnTable_NEUTRAL); |
taknokolat | 7:8989a4b84695 | 207 | if(jevoisFlag == true) FocusAdjust(); |
taknokolat | 7:8989a4b84695 | 208 | else wait(1); |
taknokolat | 7:8989a4b84695 | 209 | |
taknokolat | 7:8989a4b84695 | 210 | if(g_landingcommand!='N') return; |
taknokolat | 7:8989a4b84695 | 211 | if(!CameraDegFlag){ |
taknokolat | 7:8989a4b84695 | 212 | servoCameradeg.pulsewidth_us(1800); |
taknokolat | 7:8989a4b84695 | 213 | CameraDegFlag=!CameraDegFlag; |
taknokolat | 7:8989a4b84695 | 214 | }else{ |
taknokolat | 11:d44d137831b9 | 215 | servoCameradeg.pulsewidth_us(1400); |
taknokolat | 7:8989a4b84695 | 216 | CameraDegFlag = !CameraDegFlag; |
taknokolat | 7:8989a4b84695 | 217 | } |
taknokolat | 7:8989a4b84695 | 218 | |
taknokolat | 7:8989a4b84695 | 219 | if(jevoisFlag == true) FocusAdjust(); |
taknokolat | 7:8989a4b84695 | 220 | else wait(1); |
taknokolat | 7:8989a4b84695 | 221 | |
taknokolat | 7:8989a4b84695 | 222 | return; |
taknokolat | 7:8989a4b84695 | 223 | } |
taknokolat | 7:8989a4b84695 | 224 | |
taknokolat | 7:8989a4b84695 | 225 | void MatchPosition(){ |
taknokolat | 7:8989a4b84695 | 226 | SensingMPU(); |
taknokolat | 7:8989a4b84695 | 227 | SensingHMC(); |
taknokolat | 7:8989a4b84695 | 228 | DebugPrint(); |
taknokolat | 7:8989a4b84695 | 229 | |
taknokolat | 9:21cd5d18ad9e | 230 | while(nowAngle[YAW] <= nowAngle_HMC+5 && nowAngle[YAW] >= nowAngle_HMC-5){ |
taknokolat | 7:8989a4b84695 | 231 | MoveCansat('M'); |
taknokolat | 9:21cd5d18ad9e | 232 | SensingMPU(); |
taknokolat | 7:8989a4b84695 | 233 | } |
taknokolat | 7:8989a4b84695 | 234 | return; |
taknokolat | 7:8989a4b84695 | 235 | } |
taknokolat | 7:8989a4b84695 | 236 | |
taknokolat | 7:8989a4b84695 | 237 | void FocusAdjust(){ |
taknokolat | 11:d44d137831b9 | 238 | servoCameraPinto.pulsewidth_us(Focus_NEUTRAL-200); |
taknokolat | 11:d44d137831b9 | 239 | pc.printf("set\r\n"); |
taknokolat | 11:d44d137831b9 | 240 | wait(1); |
taknokolat | 7:8989a4b84695 | 241 | |
taknokolat | 11:d44d137831b9 | 242 | servoCameraPinto.pulsewidth_us(Focus_NEUTRAL+30); |
taknokolat | 11:d44d137831b9 | 243 | pc.printf("check\r\n"); |
taknokolat | 11:d44d137831b9 | 244 | wait(3); |
taknokolat | 11:d44d137831b9 | 245 | servoCameraPinto.pulsewidth_us(Focus_NEUTRAL); |
taknokolat | 11:d44d137831b9 | 246 | |
taknokolat | 7:8989a4b84695 | 247 | return; |
taknokolat | 7:8989a4b84695 | 248 | } |
taknokolat | 7:8989a4b84695 | 249 | |
taknokolat | 1:290e621741fd | 250 | void getSF_Serial_jevois(){ |
taknokolat | 1:290e621741fd | 251 | |
taknokolat | 1:290e621741fd | 252 | |
taknokolat | 1:290e621741fd | 253 | static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'}; |
taknokolat | 1:290e621741fd | 254 | |
taknokolat | 1:290e621741fd | 255 | static int bufcounter=0; |
taknokolat | 1:290e621741fd | 256 | |
taknokolat | 1:290e621741fd | 257 | |
taknokolat | 1:290e621741fd | 258 | |
taknokolat | 1:290e621741fd | 259 | if(pc.readable()) { // 受信確認 |
taknokolat | 1:290e621741fd | 260 | |
taknokolat | 1:290e621741fd | 261 | SFbuf[bufcounter] = pc.getc(); // 1文字取り出し |
taknokolat | 1:290e621741fd | 262 | if(SFbuf[0]!='S'){ |
taknokolat | 1:290e621741fd | 263 | //pc.printf("x"); |
taknokolat | 1:290e621741fd | 264 | return; |
taknokolat | 1:290e621741fd | 265 | } |
taknokolat | 1:290e621741fd | 266 | |
taknokolat | 1:290e621741fd | 267 | |
taknokolat | 1:290e621741fd | 268 | |
taknokolat | 1:290e621741fd | 269 | //pc.printf("%c",SFbuf[bufcounter]); |
taknokolat | 1:290e621741fd | 270 | |
Skykon | 3:c18342e4fddd | 271 | if(SFbuf[0]=='S' && bufcounter<5)bufcounter++; |
taknokolat | 1:290e621741fd | 272 | |
taknokolat | 1:290e621741fd | 273 | if(bufcounter==5 && SFbuf[4]=='F'){ |
taknokolat | 1:290e621741fd | 274 | |
taknokolat | 1:290e621741fd | 275 | g_landingcommand = SFbuf[1]; |
taknokolat | 2:f30666d7838b | 276 | wait_ms(31);//信号が速すぎることによる割り込み防止 |
taknokolat | 1:290e621741fd | 277 | //pc.printf("%c",g_landingcommand); |
taknokolat | 1:290e621741fd | 278 | //wait_ms(20); |
taknokolat | 1:290e621741fd | 279 | //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]); |
taknokolat | 1:290e621741fd | 280 | bufcounter = 0; |
taknokolat | 1:290e621741fd | 281 | memset(SFbuf, 0, sizeof(SFbuf)); |
taknokolat | 1:290e621741fd | 282 | NVIC_ClearPendingIRQ(USART2_IRQn); |
taknokolat | 1:290e621741fd | 283 | //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); |
taknokolat | 1:290e621741fd | 284 | } |
taknokolat | 1:290e621741fd | 285 | |
taknokolat | 1:290e621741fd | 286 | else if(bufcounter>=5){ |
taknokolat | 1:290e621741fd | 287 | //pc.printf("Communication Falsed.\r\n"); |
taknokolat | 1:290e621741fd | 288 | memset(SFbuf, 0, sizeof(SFbuf)); |
taknokolat | 1:290e621741fd | 289 | bufcounter = 0; |
taknokolat | 1:290e621741fd | 290 | NVIC_ClearPendingIRQ(USART2_IRQn); |
taknokolat | 1:290e621741fd | 291 | } |
taknokolat | 1:290e621741fd | 292 | } |
taknokolat | 1:290e621741fd | 293 | |
taknokolat | 1:290e621741fd | 294 | |
taknokolat | 1:290e621741fd | 295 | } |
taknokolat | 1:290e621741fd | 296 | |
taknokolat | 1:290e621741fd | 297 | |
taknokolat | 11:d44d137831b9 | 298 | /*void getSF_Serial_pi(){ |
taknokolat | 2:f30666d7838b | 299 | |
taknokolat | 7:8989a4b84695 | 300 | //NVIC_DisableIRQ(USART2_IRQn); |
taknokolat | 1:290e621741fd | 301 | |
taknokolat | 1:290e621741fd | 302 | static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'}; |
taknokolat | 1:290e621741fd | 303 | |
taknokolat | 1:290e621741fd | 304 | static int bufcounter=0; |
taknokolat | 1:290e621741fd | 305 | |
taknokolat | 1:290e621741fd | 306 | |
taknokolat | 1:290e621741fd | 307 | |
taknokolat | 1:290e621741fd | 308 | if(pc2.readable()) { // 受信確認 |
taknokolat | 1:290e621741fd | 309 | |
taknokolat | 1:290e621741fd | 310 | SFbuf[bufcounter] = pc2.getc(); // 1文字取り出し |
taknokolat | 1:290e621741fd | 311 | if(SFbuf[0]!='S'){ |
taknokolat | 1:290e621741fd | 312 | //pc.printf("x"); |
taknokolat | 1:290e621741fd | 313 | return; |
taknokolat | 1:290e621741fd | 314 | } |
taknokolat | 1:290e621741fd | 315 | |
taknokolat | 1:290e621741fd | 316 | |
taknokolat | 1:290e621741fd | 317 | |
taknokolat | 1:290e621741fd | 318 | //pc.printf("%c",SFbuf[bufcounter]); |
taknokolat | 1:290e621741fd | 319 | |
taknokolat | 1:290e621741fd | 320 | if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++; |
taknokolat | 1:290e621741fd | 321 | |
taknokolat | 1:290e621741fd | 322 | if(bufcounter==5 && SFbuf[4]=='F'){ |
taknokolat | 1:290e621741fd | 323 | |
taknokolat | 1:290e621741fd | 324 | g_landingcommand = SFbuf[1]; |
taknokolat | 2:f30666d7838b | 325 | wait_ms(31);//信号が速すぎることによる割り込み防止 |
taknokolat | 1:290e621741fd | 326 | //pc.printf("%c",g_landingcommand); |
taknokolat | 1:290e621741fd | 327 | //wait_ms(20); |
taknokolat | 1:290e621741fd | 328 | //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]); |
taknokolat | 1:290e621741fd | 329 | bufcounter = 0; |
taknokolat | 1:290e621741fd | 330 | memset(SFbuf, 0, sizeof(SFbuf)); |
taknokolat | 1:290e621741fd | 331 | NVIC_ClearPendingIRQ(USART2_IRQn); |
taknokolat | 1:290e621741fd | 332 | //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); |
taknokolat | 1:290e621741fd | 333 | } |
taknokolat | 1:290e621741fd | 334 | |
taknokolat | 1:290e621741fd | 335 | else if(bufcounter>=5){ |
taknokolat | 1:290e621741fd | 336 | //pc.printf("Communication Falsed.\r\n"); |
taknokolat | 1:290e621741fd | 337 | memset(SFbuf, 0, sizeof(SFbuf)); |
taknokolat | 1:290e621741fd | 338 | bufcounter = 0; |
taknokolat | 1:290e621741fd | 339 | NVIC_ClearPendingIRQ(USART2_IRQn); |
taknokolat | 1:290e621741fd | 340 | } |
taknokolat | 1:290e621741fd | 341 | } |
taknokolat | 2:f30666d7838b | 342 | |
taknokolat | 7:8989a4b84695 | 343 | //NVIC_EnableIRQ(USART2_IRQn); |
taknokolat | 1:290e621741fd | 344 | |
taknokolat | 11:d44d137831b9 | 345 | }*/ |
taknokolat | 1:290e621741fd | 346 | |
Skykon | 3:c18342e4fddd | 347 | |
Skykon | 3:c18342e4fddd | 348 | void setup(){ |
Skykon | 3:c18342e4fddd | 349 | |
Skykon | 3:c18342e4fddd | 350 | Init_sensors(); |
Skykon | 3:c18342e4fddd | 351 | //switch2.rise(ResetTrim); |
Skykon | 3:c18342e4fddd | 352 | |
taknokolat | 7:8989a4b84695 | 353 | //NVIC_SetPriority(USART1_IRQn,0); |
taknokolat | 7:8989a4b84695 | 354 | //NVIC_SetPriority(EXTI0_IRQn,1); |
taknokolat | 7:8989a4b84695 | 355 | //NVIC_SetPriority(TIM5_IRQn,2); |
taknokolat | 7:8989a4b84695 | 356 | //NVIC_SetPriority(EXTI9_5_IRQn,3); |
taknokolat | 7:8989a4b84695 | 357 | //NVIC_SetPriority(USART2_IRQn,4); |
taknokolat | 7:8989a4b84695 | 358 | |
taknokolat | 7:8989a4b84695 | 359 | NVIC_SetPriority(USART1_IRQn,0);//割り込み優先度 |
taknokolat | 7:8989a4b84695 | 360 | NVIC_SetPriority(USART2_IRQn,1); |
taknokolat | 7:8989a4b84695 | 361 | |
Skykon | 3:c18342e4fddd | 362 | DisplayClock(); |
Skykon | 3:c18342e4fddd | 363 | t.start(); |
Skykon | 3:c18342e4fddd | 364 | |
Skykon | 3:c18342e4fddd | 365 | pc.printf("MPU calibration start\r\n"); |
taknokolat | 6:166746820555 | 366 | pc.printf("HMC calibration start\r\n"); |
Skykon | 3:c18342e4fddd | 367 | |
Skykon | 3:c18342e4fddd | 368 | float offsetstart = t.read(); |
Skykon | 3:c18342e4fddd | 369 | while(t.read() - offsetstart < 26){ |
Skykon | 3:c18342e4fddd | 370 | SensingMPU(); |
taknokolat | 6:166746820555 | 371 | SensingHMC(); |
Skykon | 3:c18342e4fddd | 372 | for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); |
taknokolat | 6:166746820555 | 373 | pc.printf("%3.2f\t",nowAngle_HMC); |
Skykon | 3:c18342e4fddd | 374 | pc.printf("\r\n"); |
Skykon | 3:c18342e4fddd | 375 | } |
Skykon | 3:c18342e4fddd | 376 | |
Skykon | 3:c18342e4fddd | 377 | FirstROLL = nowAngle[ROLL]; |
Skykon | 3:c18342e4fddd | 378 | FirstPITCH = nowAngle[PITCH]; |
Skykon | 3:c18342e4fddd | 379 | nowAngle[ROLL] -=FirstROLL; |
Skykon | 3:c18342e4fddd | 380 | nowAngle[PITCH] -=FirstPITCH; |
taknokolat | 6:166746820555 | 381 | FirstYAW = nowAngle[YAW]; |
taknokolat | 6:166746820555 | 382 | nowAngle[YAW] -= FirstYAW; |
taknokolat | 6:166746820555 | 383 | |
taknokolat | 6:166746820555 | 384 | g_FirstYAW_HMC = nowAngle_HMC; |
taknokolat | 6:166746820555 | 385 | nowAngle_HMC -=g_FirstYAW_HMC; |
taknokolat | 6:166746820555 | 386 | if(nowAngle_HMC<0)nowAngle_HMC+=360; |
Skykon | 3:c18342e4fddd | 387 | |
Skykon | 3:c18342e4fddd | 388 | wait(0.2); |
Skykon | 3:c18342e4fddd | 389 | |
Skykon | 3:c18342e4fddd | 390 | pc.printf("All initialized\r\n"); |
taknokolat | 4:67f705d42f1e | 391 | setupFlag=true; |
taknokolat | 1:290e621741fd | 392 | } |
taknokolat | 1:290e621741fd | 393 | |
Skykon | 3:c18342e4fddd | 394 | |
Skykon | 3:c18342e4fddd | 395 | void SensingMPU(){ |
Skykon | 3:c18342e4fddd | 396 | //static int16_t deltaT = 0, t_start = 0; |
Skykon | 3:c18342e4fddd | 397 | //t_start = t.read_us(); |
Skykon | 3:c18342e4fddd | 398 | |
taknokolat | 7:8989a4b84695 | 399 | float rpy[3] = {0}; |
Skykon | 3:c18342e4fddd | 400 | static uint16_t count_changeRPY = 0; |
Skykon | 3:c18342e4fddd | 401 | static bool flg_checkoutlier = false; |
Skykon | 3:c18342e4fddd | 402 | NVIC_DisableIRQ(USART1_IRQn); |
Skykon | 3:c18342e4fddd | 403 | NVIC_DisableIRQ(USART2_IRQn); |
Skykon | 3:c18342e4fddd | 404 | NVIC_DisableIRQ(TIM5_IRQn); |
Skykon | 3:c18342e4fddd | 405 | NVIC_DisableIRQ(EXTI0_IRQn); |
Skykon | 3:c18342e4fddd | 406 | NVIC_DisableIRQ(EXTI9_5_IRQn); |
Skykon | 3:c18342e4fddd | 407 | |
Skykon | 3:c18342e4fddd | 408 | mpu6050.getRollPitchYaw_Skipper(rpy); |
Skykon | 3:c18342e4fddd | 409 | |
Skykon | 3:c18342e4fddd | 410 | NVIC_EnableIRQ(USART1_IRQn); |
Skykon | 3:c18342e4fddd | 411 | NVIC_EnableIRQ(USART2_IRQn); |
Skykon | 3:c18342e4fddd | 412 | NVIC_EnableIRQ(TIM5_IRQn); |
Skykon | 3:c18342e4fddd | 413 | NVIC_EnableIRQ(EXTI0_IRQn); |
Skykon | 3:c18342e4fddd | 414 | NVIC_EnableIRQ(EXTI9_5_IRQn); |
Skykon | 3:c18342e4fddd | 415 | |
Skykon | 3:c18342e4fddd | 416 | |
Skykon | 3:c18342e4fddd | 417 | //外れ値対策 |
Skykon | 3:c18342e4fddd | 418 | for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI; |
Skykon | 3:c18342e4fddd | 419 | rpy[ROLL] -= FirstROLL; |
Skykon | 3:c18342e4fddd | 420 | rpy[PITCH] -= FirstPITCH; |
taknokolat | 4:67f705d42f1e | 421 | if(!setupFlag){ |
taknokolat | 4:67f705d42f1e | 422 | rpy[YAW] -= FirstYAW; |
taknokolat | 4:67f705d42f1e | 423 | }else{ |
taknokolat | 4:67f705d42f1e | 424 | if(rpy[YAW] >= FirstYAW){ |
taknokolat | 4:67f705d42f1e | 425 | rpy[YAW] -= FirstYAW; |
taknokolat | 4:67f705d42f1e | 426 | }else{ |
taknokolat | 4:67f705d42f1e | 427 | rpy[YAW] += 360.0f; |
taknokolat | 4:67f705d42f1e | 428 | rpy[YAW] -= FirstYAW; |
taknokolat | 4:67f705d42f1e | 429 | } |
taknokolat | 4:67f705d42f1e | 430 | } |
Skykon | 3:c18342e4fddd | 431 | |
Skykon | 3:c18342e4fddd | 432 | for(uint8_t i=0; i<3; i++) {if(rpy[i] < nowAngle[i]-10 || rpy[i] > nowAngle[i]+10) {flg_checkoutlier = true;}} |
Skykon | 3:c18342e4fddd | 433 | if(!flg_checkoutlier || count_changeRPY >= 2){ |
Skykon | 3:c18342e4fddd | 434 | for(uint8_t i=0; i<3; i++){ |
Skykon | 3:c18342e4fddd | 435 | nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f; //2つの移動平均 |
Skykon | 3:c18342e4fddd | 436 | } |
Skykon | 3:c18342e4fddd | 437 | count_changeRPY = 0; |
Skykon | 3:c18342e4fddd | 438 | }else count_changeRPY++; |
Skykon | 3:c18342e4fddd | 439 | flg_checkoutlier = false; |
Skykon | 3:c18342e4fddd | 440 | |
Skykon | 3:c18342e4fddd | 441 | } |
Skykon | 3:c18342e4fddd | 442 | |
Skykon | 3:c18342e4fddd | 443 | |
Skykon | 3:c18342e4fddd | 444 | void Init_sensors(){ |
Skykon | 3:c18342e4fddd | 445 | if(mpu6050.setup() == -1){ |
Skykon | 3:c18342e4fddd | 446 | pc.printf("failed initialize\r\n"); |
Skykon | 3:c18342e4fddd | 447 | } |
Skykon | 3:c18342e4fddd | 448 | } |
Skykon | 3:c18342e4fddd | 449 | |
Skykon | 3:c18342e4fddd | 450 | |
Skykon | 3:c18342e4fddd | 451 | void DisplayClock(){ |
Skykon | 3:c18342e4fddd | 452 | pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000); |
Skykon | 3:c18342e4fddd | 453 | pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000); |
Skykon | 3:c18342e4fddd | 454 | pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000); |
Skykon | 3:c18342e4fddd | 455 | pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000); |
Skykon | 3:c18342e4fddd | 456 | pc.printf("\r\n"); |
taknokolat | 4:67f705d42f1e | 457 | } |
taknokolat | 6:166746820555 | 458 | |
taknokolat | 6:166746820555 | 459 | void SensingHMC(){ |
taknokolat | 6:166746820555 | 460 | //static int16_t deltaT = 0, t_start = 0; |
taknokolat | 6:166746820555 | 461 | //t_start = t.read_us(); |
taknokolat | 6:166746820555 | 462 | |
taknokolat | 7:8989a4b84695 | 463 | float rpy=0; |
taknokolat | 6:166746820555 | 464 | static uint16_t count_changeRPY = 0; |
taknokolat | 6:166746820555 | 465 | static bool flg_checkoutlier = false; |
taknokolat | 6:166746820555 | 466 | NVIC_DisableIRQ(USART1_IRQn); |
taknokolat | 6:166746820555 | 467 | NVIC_DisableIRQ(USART2_IRQn); |
taknokolat | 6:166746820555 | 468 | NVIC_DisableIRQ(TIM5_IRQn); |
taknokolat | 6:166746820555 | 469 | NVIC_DisableIRQ(EXTI0_IRQn); |
taknokolat | 6:166746820555 | 470 | NVIC_DisableIRQ(EXTI9_5_IRQn); |
taknokolat | 4:67f705d42f1e | 471 | |
taknokolat | 6:166746820555 | 472 | rpy= compass.getHeadingXYDeg(20,50); |
taknokolat | 6:166746820555 | 473 | |
taknokolat | 6:166746820555 | 474 | NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 6:166746820555 | 475 | NVIC_EnableIRQ(USART2_IRQn); |
taknokolat | 6:166746820555 | 476 | NVIC_EnableIRQ(TIM5_IRQn); |
taknokolat | 6:166746820555 | 477 | NVIC_EnableIRQ(EXTI0_IRQn); |
taknokolat | 6:166746820555 | 478 | NVIC_EnableIRQ(EXTI9_5_IRQn); |
taknokolat | 6:166746820555 | 479 | |
taknokolat | 6:166746820555 | 480 | |
taknokolat | 6:166746820555 | 481 | //外れ値対策 |
taknokolat | 6:166746820555 | 482 | //rpy*= 180.0f/PI; |
taknokolat | 6:166746820555 | 483 | if(!setupFlag){ |
taknokolat | 6:166746820555 | 484 | rpy -= g_FirstYAW_HMC; |
taknokolat | 6:166746820555 | 485 | }else{ |
taknokolat | 6:166746820555 | 486 | if(rpy >= g_FirstYAW_HMC){ |
taknokolat | 6:166746820555 | 487 | rpy -= g_FirstYAW_HMC; |
taknokolat | 6:166746820555 | 488 | }else{ |
taknokolat | 6:166746820555 | 489 | rpy += 360.0f; |
taknokolat | 6:166746820555 | 490 | rpy -= g_FirstYAW_HMC; |
taknokolat | 6:166746820555 | 491 | } |
taknokolat | 6:166746820555 | 492 | } |
taknokolat | 6:166746820555 | 493 | |
taknokolat | 6:166746820555 | 494 | if(rpy < nowAngle_HMC-10 || rpy > nowAngle_HMC+10) {flg_checkoutlier = true;} |
taknokolat | 6:166746820555 | 495 | if(!flg_checkoutlier || count_changeRPY >= 2){ |
taknokolat | 6:166746820555 | 496 | |
taknokolat | 6:166746820555 | 497 | nowAngle_HMC = (rpy + nowAngle_HMC)/2.0f; //2つの移動平均 |
taknokolat | 6:166746820555 | 498 | |
taknokolat | 6:166746820555 | 499 | count_changeRPY = 0; |
taknokolat | 6:166746820555 | 500 | }else count_changeRPY++; |
taknokolat | 6:166746820555 | 501 | flg_checkoutlier = false; |
taknokolat | 6:166746820555 | 502 | |
taknokolat | 6:166746820555 | 503 | } |
taknokolat | 6:166746820555 | 504 | |
taknokolat | 4:67f705d42f1e | 505 | void DebugPrint(){ |
taknokolat | 4:67f705d42f1e | 506 | //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); //skipper地磁気センサ_デバック用 |
taknokolat | 7:8989a4b84695 | 507 | //pc.printf("%3.2f\t",nowAngle[2]); |
taknokolat | 7:8989a4b84695 | 508 | //pc.printf("%3.2f\t",nowAngle_HMC); //HMC |
taknokolat | 7:8989a4b84695 | 509 | //pc.printf("\r\n"); |
taknokolat | 6:166746820555 | 510 | } |