SDの設定が反映されるように修正
Dependencies: mbed MPU6050_2 HMC5883L_2 SDFileSystem3
main.cpp@16:26cee2aaf61d, 2019-02-13 (annotated)
- Committer:
- taknokolat
- Date:
- Wed Feb 13 08:18:10 2019 +0000
- Revision:
- 16:26cee2aaf61d
- Parent:
- 15:1519fccfcae3
- Child:
- 17:83db05cab3d1
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" |
taknokolat | 12:083662bca47d | 4 | #include "SDFileSystem.h" |
taknokolat | 12:083662bca47d | 5 | #include "SkipperSv2.h" |
taknokolat | 12:083662bca47d | 6 | #include "falfalla.h" |
Skykon | 3:c18342e4fddd | 7 | |
Skykon | 3:c18342e4fddd | 8 | //MPU_check用 |
Skykon | 3:c18342e4fddd | 9 | #define PI 3.14159265358979 |
HARUKIDELTA | 0:84ddd6d354e1 | 10 | |
taknokolat | 11:d44d137831b9 | 11 | #define servo_NEUTRAL_R 1614 |
taknokolat | 11:d44d137831b9 | 12 | #define servo_NEUTRAL_L 1621 |
HARUKIDELTA | 0:84ddd6d354e1 | 13 | #define servo_slow_FORWARD_R 1560 |
HARUKIDELTA | 0:84ddd6d354e1 | 14 | #define servo_slow_FORWARD_L 1560 |
taknokolat | 12:083662bca47d | 15 | #define servo_low_FORWARD_R 1560 //RasPi速さ調節用 |
taknokolat | 12:083662bca47d | 16 | #define servo_high_FORWARD_R 1860 |
taknokolat | 12:083662bca47d | 17 | #define servo_low_FORWARD_L 1560 |
taknokolat | 12:083662bca47d | 18 | #define servo_high_FORWARD_L 1860 |
taknokolat | 12:083662bca47d | 19 | #define servo_low_back_R 1360 |
taknokolat | 12:083662bca47d | 20 | #define servo_high_back_R 1160 |
taknokolat | 12:083662bca47d | 21 | #define servo_low_back_L 1360 |
taknokolat | 12:083662bca47d | 22 | #define servo_high_back_L 1160 |
taknokolat | 12:083662bca47d | 23 | #define turntable_NEUTRAL 1180 //カメラ台座のサーボ |
taknokolat | 12:083662bca47d | 24 | #define matchspeed 1500 + 100 //カメラと方向を合わせるときの車輪の速度 |
taknokolat | 12:083662bca47d | 25 | #define focus_NEUTRAL 1455 //焦点合わせ用サーボ |
taknokolat | 12:083662bca47d | 26 | #define camera_deg_A 1400 //カメラ角度調整 |
taknokolat | 12:083662bca47d | 27 | #define camera_deg_B 1800 |
taknokolat | 16:26cee2aaf61d | 28 | #define pint_speed 25 |
taknokolat | 16:26cee2aaf61d | 29 | #define pint_wait 3.5 |
HARUKIDELTA | 0:84ddd6d354e1 | 30 | |
HARUKIDELTA | 0:84ddd6d354e1 | 31 | #define MOVE_NEUTRAL 0 |
HARUKIDELTA | 0:84ddd6d354e1 | 32 | #define MOVE_FORWARD 1 |
HARUKIDELTA | 0:84ddd6d354e1 | 33 | #define MOVE_LEFT 2 |
HARUKIDELTA | 0:84ddd6d354e1 | 34 | #define MOVE_RIGHT 3 |
HARUKIDELTA | 0:84ddd6d354e1 | 35 | #define MOVE_BACK 4 |
Skykon | 3:c18342e4fddd | 36 | #define GOAL_FORWARD 5 //ゴール付近_ゆっくり |
HARUKIDELTA | 0:84ddd6d354e1 | 37 | #define GOAL_LEFT 6 |
HARUKIDELTA | 0:84ddd6d354e1 | 38 | #define GOAL_RIGHT 7 |
Skykon | 3:c18342e4fddd | 39 | #define MAX_FORWARD 8 //はやい_姿勢修正用 |
HARUKIDELTA | 0:84ddd6d354e1 | 40 | #define MAX_BACK 9 |
HARUKIDELTA | 0:84ddd6d354e1 | 41 | |
taknokolat | 1:290e621741fd | 42 | void getSF_Serial_jevois(); |
taknokolat | 1:290e621741fd | 43 | void getSF_Serial_pi(); |
taknokolat | 1:290e621741fd | 44 | |
Skykon | 3:c18342e4fddd | 45 | //MPU_check用 |
Skykon | 3:c18342e4fddd | 46 | void SensingMPU(); |
taknokolat | 7:8989a4b84695 | 47 | |
taknokolat | 7:8989a4b84695 | 48 | void MoveCansat(char g_landingcommand); |
Skykon | 3:c18342e4fddd | 49 | void setup(); |
Skykon | 3:c18342e4fddd | 50 | void Init_sensors(); |
Skykon | 3:c18342e4fddd | 51 | void DisplayClock(); |
taknokolat | 4:67f705d42f1e | 52 | void DebugPrint(); |
taknokolat | 6:166746820555 | 53 | void SensingHMC(); |
taknokolat | 7:8989a4b84695 | 54 | void MoveCameraBoard(); |
taknokolat | 7:8989a4b84695 | 55 | void MatchPosition(); |
taknokolat | 7:8989a4b84695 | 56 | void FocusAdjust(); |
Skykon | 3:c18342e4fddd | 57 | |
taknokolat | 12:083662bca47d | 58 | //sd設定 |
taknokolat | 12:083662bca47d | 59 | int GetParameter(FILE *fp, const char *paramName,char parameter[]); |
taknokolat | 12:083662bca47d | 60 | |
taknokolat | 13:b088f0db7158 | 61 | int SetOptions(int *Servo_NEUTRAL_R,int *Servo_NEUTRAL_L, |
taknokolat | 12:083662bca47d | 62 | int *Servo_high_FORWARD_R,int *Servo_high_FORWARD_L, |
taknokolat | 12:083662bca47d | 63 | int *Servo_slow_FORWARD_R,int *Servo_slow_FORWARD_L, |
taknokolat | 12:083662bca47d | 64 | int *Turntable_NEUTRAL,int *Matchspeed,int *Focus_NEUTRAL, |
taknokolat | 16:26cee2aaf61d | 65 | int *Camera_deg_A, int *Camera_deg_B, |
taknokolat | 16:26cee2aaf61d | 66 | int *Pint_speed, float *Pint_wait |
taknokolat | 12:083662bca47d | 67 | ); |
taknokolat | 12:083662bca47d | 68 | |
taknokolat | 6:166746820555 | 69 | static float nowAngle[3] = {0,0,0},nowAngle_HMC=0; |
taknokolat | 6:166746820555 | 70 | float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0,g_FirstYAW_HMC; |
Skykon | 3:c18342e4fddd | 71 | |
taknokolat | 7:8989a4b84695 | 72 | bool setupFlag = false; |
taknokolat | 7:8989a4b84695 | 73 | bool CameraDegFlag = false; |
taknokolat | 7:8989a4b84695 | 74 | bool jevoisFlag = true; |
taknokolat | 4:67f705d42f1e | 75 | |
Skykon | 3:c18342e4fddd | 76 | enum Angle{ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度 |
Skykon | 3:c18342e4fddd | 77 | |
Skykon | 3:c18342e4fddd | 78 | Timer t; |
Skykon | 3:c18342e4fddd | 79 | |
taknokolat | 14:90e9b44eb819 | 80 | SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd"); |
taknokolat | 14:90e9b44eb819 | 81 | |
taknokolat | 11:d44d137831b9 | 82 | RawSerial pc(PA_2,PA_3,115200); //uart2 |
taknokolat | 11:d44d137831b9 | 83 | //pa2:UART2_TX,pa3:UART2_RX |
taknokolat | 12:083662bca47d | 84 | RawSerial pc2(PA_11,PA_12,115200); //uart1 |
taknokolat | 11:d44d137831b9 | 85 | //pb6:UART1_TX,pb7:UART1_RX |
taknokolat | 11:d44d137831b9 | 86 | |
HARUKIDELTA | 5:8bfe95431ec0 | 87 | //PWM pin宣言 |
taknokolat | 6:166746820555 | 88 | PwmOut servoR(PC_6); //TIM3_CH1 車輪右 |
taknokolat | 6:166746820555 | 89 | PwmOut servoL(PC_7); //TIM3_CH2 車輪左 |
HARUKIDELTA | 5:8bfe95431ec0 | 90 | PwmOut servoTurnTable(PB_0); //TIM3_CH3 カメラ台回転Servo |
HARUKIDELTA | 5:8bfe95431ec0 | 91 | PwmOut servoCameradeg(PB_1); //TIM3_CH4 カメラ角度調節Servo |
HARUKIDELTA | 5:8bfe95431ec0 | 92 | PwmOut servoCameraPinto(PB_6); //TIM4_CH1 カメラピント合わせ |
HARUKIDELTA | 5:8bfe95431ec0 | 93 | PwmOut servoCameramount(PA_6); //skipperウラ カメラマウント起動 |
HARUKIDELTA | 5:8bfe95431ec0 | 94 | PwmOut servoGetUP(PC_8); //skipperウラ 起き上がり動作 |
Skykon | 3:c18342e4fddd | 95 | |
HARUKIDELTA | 5:8bfe95431ec0 | 96 | /*通信用のpinは |
HARUKIDELTA | 5:8bfe95431ec0 | 97 | PA_3(UART2_Rx) skipperウラ |
HARUKIDELTA | 5:8bfe95431ec0 | 98 | PA_12(UART6_Rx) skipperオモテ USB端子より |
HARUKIDELTA | 5:8bfe95431ec0 | 99 | PB_7 (UART1_Rx) skipperオモテ 6番目 TIM4_CH2 |
HARUKIDELTA | 5:8bfe95431ec0 | 100 | */ |
HARUKIDELTA | 5:8bfe95431ec0 | 101 | |
HARUKIDELTA | 5:8bfe95431ec0 | 102 | /*超音波はRaspberryPiに積む*/ |
HARUKIDELTA | 5:8bfe95431ec0 | 103 | |
HARUKIDELTA | 5:8bfe95431ec0 | 104 | //外付けコンパス |
taknokolat | 6:166746820555 | 105 | HMC5883L compass(PB_9, PB_8); //コンパスセンサー TIM4_CH3とCH4 |
HARUKIDELTA | 0:84ddd6d354e1 | 106 | |
taknokolat | 2:f30666d7838b | 107 | char g_landingcommand='N'; |
taknokolat | 1:290e621741fd | 108 | |
Skykon | 3:c18342e4fddd | 109 | //MPU_check用 |
Skykon | 3:c18342e4fddd | 110 | MPU6050DMP6 mpu6050(PC_0,&pc); |
Skykon | 3:c18342e4fddd | 111 | |
Skykon | 3:c18342e4fddd | 112 | |
HARUKIDELTA | 0:84ddd6d354e1 | 113 | int main() { |
taknokolat | 1:290e621741fd | 114 | |
Skykon | 3:c18342e4fddd | 115 | //MPU_check |
Skykon | 3:c18342e4fddd | 116 | setup(); |
Skykon | 3:c18342e4fddd | 117 | |
taknokolat | 15:1519fccfcae3 | 118 | servoCameradeg.pulsewidth_us(camera_deg_A); |
taknokolat | 11:d44d137831b9 | 119 | |
taknokolat | 1:290e621741fd | 120 | // シリアル通信受信の割り込みイベント登録 |
taknokolat | 1:290e621741fd | 121 | pc.attach(getSF_Serial_jevois, Serial::RxIrq); |
taknokolat | 12:083662bca47d | 122 | pc2.attach(getSF_Serial_pi, Serial::RxIrq); |
taknokolat | 1:290e621741fd | 123 | |
HARUKIDELTA | 0:84ddd6d354e1 | 124 | while(1) { |
taknokolat | 7:8989a4b84695 | 125 | if(jevoisFlag==true) NVIC_DisableIRQ(USART1_IRQn); |
taknokolat | 7:8989a4b84695 | 126 | else NVIC_DisableIRQ(USART2_IRQn); |
taknokolat | 8:d11a59d2a2f1 | 127 | |
taknokolat | 10:63fe920595a7 | 128 | MoveCameraBoard(); |
taknokolat | 10:63fe920595a7 | 129 | |
taknokolat | 7:8989a4b84695 | 130 | if(g_landingcommand!='N') MatchPosition(); |
taknokolat | 8:d11a59d2a2f1 | 131 | |
taknokolat | 8:d11a59d2a2f1 | 132 | if(jevoisFlag==true) NVIC_DisableIRQ(USART1_IRQn); |
taknokolat | 8:d11a59d2a2f1 | 133 | else NVIC_DisableIRQ(USART2_IRQn); |
taknokolat | 8:d11a59d2a2f1 | 134 | |
taknokolat | 7:8989a4b84695 | 135 | MoveCansat(g_landingcommand); |
Skykon | 3:c18342e4fddd | 136 | wait_ms(23); |
taknokolat | 8:d11a59d2a2f1 | 137 | |
taknokolat | 7:8989a4b84695 | 138 | if(jevoisFlag==true) NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 7:8989a4b84695 | 139 | else NVIC_EnableIRQ(USART2_IRQn); |
HARUKIDELTA | 0:84ddd6d354e1 | 140 | } |
HARUKIDELTA | 0:84ddd6d354e1 | 141 | } |
HARUKIDELTA | 0:84ddd6d354e1 | 142 | |
Skykon | 3:c18342e4fddd | 143 | |
taknokolat | 1:290e621741fd | 144 | void MoveCansat(char g_landingcommand) |
HARUKIDELTA | 0:84ddd6d354e1 | 145 | { |
taknokolat | 2:f30666d7838b | 146 | //NVIC_DisableIRQ(USART1_IRQn); |
taknokolat | 7:8989a4b84695 | 147 | //NVIC_DisableIRQ(USART2_IRQn); |
taknokolat | 1:290e621741fd | 148 | switch(g_landingcommand){ |
taknokolat | 2:f30666d7838b | 149 | case 'N': //MOVE_NEUTRAL |
HARUKIDELTA | 0:84ddd6d354e1 | 150 | servoR.pulsewidth_us(servo_NEUTRAL_R); |
HARUKIDELTA | 0:84ddd6d354e1 | 151 | servoL.pulsewidth_us(servo_NEUTRAL_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 | 2:f30666d7838b | 156 | case 'Y': //MOVE_FORWARD |
taknokolat | 12:083662bca47d | 157 | servoR.pulsewidth_us(servo_high_FORWARD_R); |
taknokolat | 12:083662bca47d | 158 | servoL.pulsewidth_us(servo_high_FORWARD_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 | 12:083662bca47d | 163 | case 'l': //MOVE_LEFT Low Speed |
taknokolat | 12:083662bca47d | 164 | servoR.pulsewidth_us(servo_low_FORWARD_R); |
taknokolat | 12:083662bca47d | 165 | //NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 12:083662bca47d | 166 | //NVIC_EnableIRQ(USART2_IRQn); |
taknokolat | 12:083662bca47d | 167 | |
taknokolat | 12:083662bca47d | 168 | case 'L': //MOVE_LEFT High Speed |
taknokolat | 12:083662bca47d | 169 | servoR.pulsewidth_us(servo_high_FORWARD_R); |
taknokolat | 2:f30666d7838b | 170 | //NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 7:8989a4b84695 | 171 | //NVIC_EnableIRQ(USART2_IRQn); |
taknokolat | 1:290e621741fd | 172 | |
taknokolat | 12:083662bca47d | 173 | case 'r': //MOVE_RIGHT Low Speed |
taknokolat | 12:083662bca47d | 174 | servoL.pulsewidth_us(servo_low_FORWARD_L); |
taknokolat | 2:f30666d7838b | 175 | //NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 7:8989a4b84695 | 176 | //NVIC_EnableIRQ(USART2_IRQn); |
HARUKIDELTA | 0:84ddd6d354e1 | 177 | break; |
taknokolat | 12:083662bca47d | 178 | |
taknokolat | 12:083662bca47d | 179 | case 'R': //MOVE_RIGHT High Speed |
taknokolat | 12:083662bca47d | 180 | servoL.pulsewidth_us(servo_high_FORWARD_L); |
taknokolat | 2:f30666d7838b | 181 | //NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 7:8989a4b84695 | 182 | //NVIC_EnableIRQ(USART2_IRQn); |
HARUKIDELTA | 0:84ddd6d354e1 | 183 | break; |
taknokolat | 1:290e621741fd | 184 | |
taknokolat | 1:290e621741fd | 185 | case 'G': //GOAL_FORWARD |
HARUKIDELTA | 0:84ddd6d354e1 | 186 | servoR.pulsewidth_us(servo_slow_FORWARD_R); |
HARUKIDELTA | 0:84ddd6d354e1 | 187 | servoL.pulsewidth_us(servo_slow_FORWARD_L); |
taknokolat | 2:f30666d7838b | 188 | //NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 7:8989a4b84695 | 189 | //NVIC_EnableIRQ(USART2_IRQn); |
taknokolat | 7:8989a4b84695 | 190 | break; |
taknokolat | 12:083662bca47d | 191 | |
taknokolat | 12:083662bca47d | 192 | case '1': //MOVE_FORWARD Speed Level 1 |
taknokolat | 12:083662bca47d | 193 | servoR.pulsewidth_us(servo_high_FORWARD_R); |
taknokolat | 12:083662bca47d | 194 | servoL.pulsewidth_us(servo_high_FORWARD_L); |
taknokolat | 12:083662bca47d | 195 | //NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 12:083662bca47d | 196 | wait(1); |
taknokolat | 12:083662bca47d | 197 | do{ |
taknokolat | 12:083662bca47d | 198 | }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20)); |
taknokolat | 12:083662bca47d | 199 | //NVIC_EnableIRQ(USART2_IRQn); |
taknokolat | 12:083662bca47d | 200 | break; |
taknokolat | 12:083662bca47d | 201 | |
taknokolat | 12:083662bca47d | 202 | case '2': //MOVE_FORWARD Speed Level 2 |
taknokolat | 12:083662bca47d | 203 | servoR.pulsewidth_us(servo_high_FORWARD_R); |
taknokolat | 12:083662bca47d | 204 | servoL.pulsewidth_us(servo_high_FORWARD_L); |
taknokolat | 12:083662bca47d | 205 | //NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 12:083662bca47d | 206 | wait(2); |
taknokolat | 12:083662bca47d | 207 | do{ |
taknokolat | 12:083662bca47d | 208 | }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20)); |
taknokolat | 12:083662bca47d | 209 | //NVIC_EnableIRQ(USART2_IRQn); |
taknokolat | 12:083662bca47d | 210 | break; |
taknokolat | 12:083662bca47d | 211 | |
taknokolat | 12:083662bca47d | 212 | case '3': //MOVE_FORWARD Speed Level 3 |
taknokolat | 12:083662bca47d | 213 | servoR.pulsewidth_us(servo_high_FORWARD_R); |
taknokolat | 12:083662bca47d | 214 | servoL.pulsewidth_us(servo_high_FORWARD_L); |
taknokolat | 12:083662bca47d | 215 | //NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 12:083662bca47d | 216 | wait(3); |
taknokolat | 12:083662bca47d | 217 | do{ |
taknokolat | 12:083662bca47d | 218 | }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20)); |
taknokolat | 12:083662bca47d | 219 | //NVIC_EnableIRQ(USART2_IRQn); |
taknokolat | 12:083662bca47d | 220 | break; |
taknokolat | 12:083662bca47d | 221 | |
taknokolat | 12:083662bca47d | 222 | case '4': //MOVE_FORWARD Speed Level 4 |
taknokolat | 12:083662bca47d | 223 | servoR.pulsewidth_us(servo_high_FORWARD_R); |
taknokolat | 12:083662bca47d | 224 | servoL.pulsewidth_us(servo_high_FORWARD_L); |
taknokolat | 12:083662bca47d | 225 | //NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 12:083662bca47d | 226 | wait(4); |
taknokolat | 12:083662bca47d | 227 | do{ |
taknokolat | 12:083662bca47d | 228 | }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20)); |
taknokolat | 12:083662bca47d | 229 | //NVIC_EnableIRQ(USART2_IRQn); |
taknokolat | 12:083662bca47d | 230 | break; |
taknokolat | 7:8989a4b84695 | 231 | |
taknokolat | 12:083662bca47d | 232 | case '5': //MOVE_FORWARD Speed Level 5 |
taknokolat | 12:083662bca47d | 233 | servoR.pulsewidth_us(servo_high_FORWARD_R); |
taknokolat | 12:083662bca47d | 234 | servoL.pulsewidth_us(servo_high_FORWARD_L); |
taknokolat | 7:8989a4b84695 | 235 | //NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 7:8989a4b84695 | 236 | wait(5); |
taknokolat | 10:63fe920595a7 | 237 | do{ |
taknokolat | 10:63fe920595a7 | 238 | }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20)); |
taknokolat | 7:8989a4b84695 | 239 | //NVIC_EnableIRQ(USART2_IRQn); |
taknokolat | 7:8989a4b84695 | 240 | break; |
taknokolat | 7:8989a4b84695 | 241 | |
taknokolat | 7:8989a4b84695 | 242 | case 'M': //MatchPosition |
taknokolat | 12:083662bca47d | 243 | servoR.pulsewidth_us(matchspeed); |
taknokolat | 7:8989a4b84695 | 244 | //NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 7:8989a4b84695 | 245 | //NVIC_EnableIRQ(USART2_IRQn); |
taknokolat | 7:8989a4b84695 | 246 | break; |
taknokolat | 7:8989a4b84695 | 247 | |
taknokolat | 12:083662bca47d | 248 | case 'P': //jevoisからraspberry piへの切り替え |
taknokolat | 7:8989a4b84695 | 249 | jevoisFlag = false; |
taknokolat | 7:8989a4b84695 | 250 | //NVIC_EnableIRQ(USART2_IRQn); |
HARUKIDELTA | 0:84ddd6d354e1 | 251 | break; |
taknokolat | 12:083662bca47d | 252 | |
taknokolat | 12:083662bca47d | 253 | case 'S': |
taknokolat | 12:083662bca47d | 254 | /*RasPiからの超音波判定(プログラムスタート部)*/ |
taknokolat | 12:083662bca47d | 255 | break; |
taknokolat | 1:290e621741fd | 256 | |
taknokolat | 1:290e621741fd | 257 | default : |
taknokolat | 2:f30666d7838b | 258 | //NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 7:8989a4b84695 | 259 | //NVIC_EnableIRQ(USART2_IRQn); |
HARUKIDELTA | 0:84ddd6d354e1 | 260 | break; |
taknokolat | 1:290e621741fd | 261 | |
HARUKIDELTA | 0:84ddd6d354e1 | 262 | } |
taknokolat | 7:8989a4b84695 | 263 | |
HARUKIDELTA | 0:84ddd6d354e1 | 264 | return; |
taknokolat | 1:290e621741fd | 265 | } |
taknokolat | 1:290e621741fd | 266 | |
taknokolat | 7:8989a4b84695 | 267 | void MoveCameraBoard(){ |
taknokolat | 7:8989a4b84695 | 268 | MoveCansat('N'); |
taknokolat | 10:63fe920595a7 | 269 | g_landingcommand='N'; |
taknokolat | 11:d44d137831b9 | 270 | servoTurnTable.pulsewidth_us(1800); |
taknokolat | 11:d44d137831b9 | 271 | wait_ms(600); |
taknokolat | 12:083662bca47d | 272 | servoTurnTable.pulsewidth_us(turntable_NEUTRAL); |
taknokolat | 7:8989a4b84695 | 273 | if(jevoisFlag == true) FocusAdjust(); |
taknokolat | 7:8989a4b84695 | 274 | else wait(1); |
taknokolat | 7:8989a4b84695 | 275 | |
taknokolat | 7:8989a4b84695 | 276 | if(g_landingcommand!='N') return; |
taknokolat | 7:8989a4b84695 | 277 | if(!CameraDegFlag){ |
taknokolat | 15:1519fccfcae3 | 278 | for(int i=0; i<camera_deg_B-camera_deg_A;i=i+10){ |
taknokolat | 15:1519fccfcae3 | 279 | servoCameradeg.pulsewidth_us(camera_deg_A+10); |
taknokolat | 15:1519fccfcae3 | 280 | wait_ms(20); |
taknokolat | 15:1519fccfcae3 | 281 | } |
taknokolat | 7:8989a4b84695 | 282 | CameraDegFlag=!CameraDegFlag; |
taknokolat | 7:8989a4b84695 | 283 | }else{ |
taknokolat | 15:1519fccfcae3 | 284 | for(int i=0; i<camera_deg_B-camera_deg_A;i=i+10){ |
taknokolat | 15:1519fccfcae3 | 285 | servoCameradeg.pulsewidth_us(camera_deg_A-10); |
taknokolat | 15:1519fccfcae3 | 286 | } |
taknokolat | 7:8989a4b84695 | 287 | CameraDegFlag = !CameraDegFlag; |
taknokolat | 7:8989a4b84695 | 288 | } |
taknokolat | 7:8989a4b84695 | 289 | |
taknokolat | 7:8989a4b84695 | 290 | if(jevoisFlag == true) FocusAdjust(); |
taknokolat | 7:8989a4b84695 | 291 | else wait(1); |
taknokolat | 7:8989a4b84695 | 292 | |
taknokolat | 7:8989a4b84695 | 293 | return; |
taknokolat | 7:8989a4b84695 | 294 | } |
taknokolat | 7:8989a4b84695 | 295 | |
taknokolat | 7:8989a4b84695 | 296 | void MatchPosition(){ |
taknokolat | 7:8989a4b84695 | 297 | SensingMPU(); |
taknokolat | 7:8989a4b84695 | 298 | SensingHMC(); |
taknokolat | 7:8989a4b84695 | 299 | DebugPrint(); |
taknokolat | 7:8989a4b84695 | 300 | |
taknokolat | 9:21cd5d18ad9e | 301 | while(nowAngle[YAW] <= nowAngle_HMC+5 && nowAngle[YAW] >= nowAngle_HMC-5){ |
taknokolat | 7:8989a4b84695 | 302 | MoveCansat('M'); |
taknokolat | 9:21cd5d18ad9e | 303 | SensingMPU(); |
taknokolat | 7:8989a4b84695 | 304 | } |
taknokolat | 7:8989a4b84695 | 305 | return; |
taknokolat | 7:8989a4b84695 | 306 | } |
taknokolat | 7:8989a4b84695 | 307 | |
taknokolat | 7:8989a4b84695 | 308 | void FocusAdjust(){ |
taknokolat | 15:1519fccfcae3 | 309 | servoCameraPinto.pulsewidth_us(focus_NEUTRAL-200); |
taknokolat | 15:1519fccfcae3 | 310 | //pc.printf("set\r\n"); |
taknokolat | 11:d44d137831b9 | 311 | wait(1); |
taknokolat | 7:8989a4b84695 | 312 | |
taknokolat | 16:26cee2aaf61d | 313 | servoCameraPinto.pulsewidth_us(focus_NEUTRAL+pint_speed); |
taknokolat | 15:1519fccfcae3 | 314 | //pc.printf("check\r\n"); |
taknokolat | 16:26cee2aaf61d | 315 | wait(pint_wait); |
taknokolat | 15:1519fccfcae3 | 316 | servoCameraPinto.pulsewidth_us(focus_NEUTRAL); |
taknokolat | 11:d44d137831b9 | 317 | |
taknokolat | 7:8989a4b84695 | 318 | return; |
taknokolat | 15:1519fccfcae3 | 319 | } |
taknokolat | 7:8989a4b84695 | 320 | |
taknokolat | 1:290e621741fd | 321 | void getSF_Serial_jevois(){ |
taknokolat | 1:290e621741fd | 322 | |
taknokolat | 1:290e621741fd | 323 | |
taknokolat | 1:290e621741fd | 324 | static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'}; |
taknokolat | 1:290e621741fd | 325 | |
taknokolat | 1:290e621741fd | 326 | static int bufcounter=0; |
taknokolat | 1:290e621741fd | 327 | |
taknokolat | 1:290e621741fd | 328 | |
taknokolat | 1:290e621741fd | 329 | |
taknokolat | 1:290e621741fd | 330 | if(pc.readable()) { // 受信確認 |
taknokolat | 1:290e621741fd | 331 | |
taknokolat | 1:290e621741fd | 332 | SFbuf[bufcounter] = pc.getc(); // 1文字取り出し |
taknokolat | 1:290e621741fd | 333 | if(SFbuf[0]!='S'){ |
taknokolat | 1:290e621741fd | 334 | //pc.printf("x"); |
taknokolat | 1:290e621741fd | 335 | return; |
taknokolat | 1:290e621741fd | 336 | } |
taknokolat | 1:290e621741fd | 337 | |
taknokolat | 1:290e621741fd | 338 | |
taknokolat | 1:290e621741fd | 339 | |
taknokolat | 1:290e621741fd | 340 | //pc.printf("%c",SFbuf[bufcounter]); |
taknokolat | 1:290e621741fd | 341 | |
Skykon | 3:c18342e4fddd | 342 | if(SFbuf[0]=='S' && bufcounter<5)bufcounter++; |
taknokolat | 1:290e621741fd | 343 | |
taknokolat | 1:290e621741fd | 344 | if(bufcounter==5 && SFbuf[4]=='F'){ |
taknokolat | 1:290e621741fd | 345 | |
taknokolat | 1:290e621741fd | 346 | g_landingcommand = SFbuf[1]; |
taknokolat | 2:f30666d7838b | 347 | wait_ms(31);//信号が速すぎることによる割り込み防止 |
taknokolat | 1:290e621741fd | 348 | //pc.printf("%c",g_landingcommand); |
taknokolat | 1:290e621741fd | 349 | //wait_ms(20); |
taknokolat | 1:290e621741fd | 350 | //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]); |
taknokolat | 1:290e621741fd | 351 | bufcounter = 0; |
taknokolat | 1:290e621741fd | 352 | memset(SFbuf, 0, sizeof(SFbuf)); |
taknokolat | 1:290e621741fd | 353 | NVIC_ClearPendingIRQ(USART2_IRQn); |
taknokolat | 1:290e621741fd | 354 | //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); |
taknokolat | 1:290e621741fd | 355 | } |
taknokolat | 1:290e621741fd | 356 | |
taknokolat | 1:290e621741fd | 357 | else if(bufcounter>=5){ |
taknokolat | 1:290e621741fd | 358 | //pc.printf("Communication Falsed.\r\n"); |
taknokolat | 1:290e621741fd | 359 | memset(SFbuf, 0, sizeof(SFbuf)); |
taknokolat | 1:290e621741fd | 360 | bufcounter = 0; |
taknokolat | 1:290e621741fd | 361 | NVIC_ClearPendingIRQ(USART2_IRQn); |
taknokolat | 1:290e621741fd | 362 | } |
taknokolat | 1:290e621741fd | 363 | } |
taknokolat | 1:290e621741fd | 364 | |
taknokolat | 1:290e621741fd | 365 | |
taknokolat | 1:290e621741fd | 366 | } |
taknokolat | 1:290e621741fd | 367 | |
taknokolat | 1:290e621741fd | 368 | |
taknokolat | 12:083662bca47d | 369 | void getSF_Serial_pi(){ |
taknokolat | 2:f30666d7838b | 370 | |
taknokolat | 7:8989a4b84695 | 371 | //NVIC_DisableIRQ(USART2_IRQn); |
taknokolat | 1:290e621741fd | 372 | |
taknokolat | 1:290e621741fd | 373 | static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'}; |
taknokolat | 1:290e621741fd | 374 | |
taknokolat | 1:290e621741fd | 375 | static int bufcounter=0; |
taknokolat | 1:290e621741fd | 376 | |
taknokolat | 1:290e621741fd | 377 | |
taknokolat | 1:290e621741fd | 378 | |
taknokolat | 1:290e621741fd | 379 | if(pc2.readable()) { // 受信確認 |
taknokolat | 1:290e621741fd | 380 | |
taknokolat | 1:290e621741fd | 381 | SFbuf[bufcounter] = pc2.getc(); // 1文字取り出し |
taknokolat | 1:290e621741fd | 382 | if(SFbuf[0]!='S'){ |
taknokolat | 1:290e621741fd | 383 | //pc.printf("x"); |
taknokolat | 1:290e621741fd | 384 | return; |
taknokolat | 1:290e621741fd | 385 | } |
taknokolat | 1:290e621741fd | 386 | |
taknokolat | 1:290e621741fd | 387 | |
taknokolat | 1:290e621741fd | 388 | |
taknokolat | 1:290e621741fd | 389 | //pc.printf("%c",SFbuf[bufcounter]); |
taknokolat | 1:290e621741fd | 390 | |
taknokolat | 1:290e621741fd | 391 | if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++; |
taknokolat | 1:290e621741fd | 392 | |
taknokolat | 1:290e621741fd | 393 | if(bufcounter==5 && SFbuf[4]=='F'){ |
taknokolat | 1:290e621741fd | 394 | |
taknokolat | 1:290e621741fd | 395 | g_landingcommand = SFbuf[1]; |
taknokolat | 2:f30666d7838b | 396 | wait_ms(31);//信号が速すぎることによる割り込み防止 |
taknokolat | 1:290e621741fd | 397 | //pc.printf("%c",g_landingcommand); |
taknokolat | 1:290e621741fd | 398 | //wait_ms(20); |
taknokolat | 1:290e621741fd | 399 | //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]); |
taknokolat | 1:290e621741fd | 400 | bufcounter = 0; |
taknokolat | 1:290e621741fd | 401 | memset(SFbuf, 0, sizeof(SFbuf)); |
taknokolat | 1:290e621741fd | 402 | NVIC_ClearPendingIRQ(USART2_IRQn); |
taknokolat | 1:290e621741fd | 403 | //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); |
taknokolat | 1:290e621741fd | 404 | } |
taknokolat | 1:290e621741fd | 405 | |
taknokolat | 1:290e621741fd | 406 | else if(bufcounter>=5){ |
taknokolat | 1:290e621741fd | 407 | //pc.printf("Communication Falsed.\r\n"); |
taknokolat | 1:290e621741fd | 408 | memset(SFbuf, 0, sizeof(SFbuf)); |
taknokolat | 1:290e621741fd | 409 | bufcounter = 0; |
taknokolat | 1:290e621741fd | 410 | NVIC_ClearPendingIRQ(USART2_IRQn); |
taknokolat | 1:290e621741fd | 411 | } |
taknokolat | 1:290e621741fd | 412 | } |
taknokolat | 2:f30666d7838b | 413 | |
taknokolat | 7:8989a4b84695 | 414 | //NVIC_EnableIRQ(USART2_IRQn); |
taknokolat | 1:290e621741fd | 415 | |
taknokolat | 12:083662bca47d | 416 | } |
taknokolat | 1:290e621741fd | 417 | |
Skykon | 3:c18342e4fddd | 418 | |
Skykon | 3:c18342e4fddd | 419 | void setup(){ |
Skykon | 3:c18342e4fddd | 420 | |
taknokolat | 13:b088f0db7158 | 421 | SetOptions(&Servo_NEUTRAL_R, &Servo_NEUTRAL_L, |
taknokolat | 12:083662bca47d | 422 | &Servo_high_FORWARD_R, &Servo_high_FORWARD_L, |
taknokolat | 12:083662bca47d | 423 | &Servo_slow_FORWARD_R,&Servo_slow_FORWARD_L, |
taknokolat | 12:083662bca47d | 424 | &Turntable_NEUTRAL,&Matchspeed,&Focus_NEUTRAL, |
taknokolat | 16:26cee2aaf61d | 425 | &Camera_deg_A, &Camera_deg_B, |
taknokolat | 16:26cee2aaf61d | 426 | &Pint_speed,&Pint_wait |
taknokolat | 16:26cee2aaf61d | 427 | |
taknokolat | 13:b088f0db7158 | 428 | ); |
taknokolat | 12:083662bca47d | 429 | |
Skykon | 3:c18342e4fddd | 430 | Init_sensors(); |
Skykon | 3:c18342e4fddd | 431 | //switch2.rise(ResetTrim); |
Skykon | 3:c18342e4fddd | 432 | |
taknokolat | 7:8989a4b84695 | 433 | //NVIC_SetPriority(USART1_IRQn,0); |
taknokolat | 7:8989a4b84695 | 434 | //NVIC_SetPriority(EXTI0_IRQn,1); |
taknokolat | 7:8989a4b84695 | 435 | //NVIC_SetPriority(TIM5_IRQn,2); |
taknokolat | 7:8989a4b84695 | 436 | //NVIC_SetPriority(EXTI9_5_IRQn,3); |
taknokolat | 7:8989a4b84695 | 437 | //NVIC_SetPriority(USART2_IRQn,4); |
taknokolat | 7:8989a4b84695 | 438 | |
taknokolat | 7:8989a4b84695 | 439 | NVIC_SetPriority(USART1_IRQn,0);//割り込み優先度 |
taknokolat | 7:8989a4b84695 | 440 | NVIC_SetPriority(USART2_IRQn,1); |
taknokolat | 7:8989a4b84695 | 441 | |
Skykon | 3:c18342e4fddd | 442 | DisplayClock(); |
Skykon | 3:c18342e4fddd | 443 | t.start(); |
Skykon | 3:c18342e4fddd | 444 | |
Skykon | 3:c18342e4fddd | 445 | pc.printf("MPU calibration start\r\n"); |
taknokolat | 6:166746820555 | 446 | pc.printf("HMC calibration start\r\n"); |
Skykon | 3:c18342e4fddd | 447 | |
Skykon | 3:c18342e4fddd | 448 | float offsetstart = t.read(); |
Skykon | 3:c18342e4fddd | 449 | while(t.read() - offsetstart < 26){ |
Skykon | 3:c18342e4fddd | 450 | SensingMPU(); |
taknokolat | 6:166746820555 | 451 | SensingHMC(); |
Skykon | 3:c18342e4fddd | 452 | for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); |
taknokolat | 6:166746820555 | 453 | pc.printf("%3.2f\t",nowAngle_HMC); |
Skykon | 3:c18342e4fddd | 454 | pc.printf("\r\n"); |
Skykon | 3:c18342e4fddd | 455 | } |
Skykon | 3:c18342e4fddd | 456 | |
Skykon | 3:c18342e4fddd | 457 | FirstROLL = nowAngle[ROLL]; |
Skykon | 3:c18342e4fddd | 458 | FirstPITCH = nowAngle[PITCH]; |
Skykon | 3:c18342e4fddd | 459 | nowAngle[ROLL] -=FirstROLL; |
Skykon | 3:c18342e4fddd | 460 | nowAngle[PITCH] -=FirstPITCH; |
taknokolat | 6:166746820555 | 461 | FirstYAW = nowAngle[YAW]; |
taknokolat | 6:166746820555 | 462 | nowAngle[YAW] -= FirstYAW; |
taknokolat | 6:166746820555 | 463 | |
taknokolat | 6:166746820555 | 464 | g_FirstYAW_HMC = nowAngle_HMC; |
taknokolat | 6:166746820555 | 465 | nowAngle_HMC -=g_FirstYAW_HMC; |
taknokolat | 6:166746820555 | 466 | if(nowAngle_HMC<0)nowAngle_HMC+=360; |
Skykon | 3:c18342e4fddd | 467 | |
Skykon | 3:c18342e4fddd | 468 | wait(0.2); |
Skykon | 3:c18342e4fddd | 469 | |
Skykon | 3:c18342e4fddd | 470 | pc.printf("All initialized\r\n"); |
taknokolat | 4:67f705d42f1e | 471 | setupFlag=true; |
taknokolat | 1:290e621741fd | 472 | } |
taknokolat | 1:290e621741fd | 473 | |
Skykon | 3:c18342e4fddd | 474 | |
Skykon | 3:c18342e4fddd | 475 | void SensingMPU(){ |
Skykon | 3:c18342e4fddd | 476 | //static int16_t deltaT = 0, t_start = 0; |
Skykon | 3:c18342e4fddd | 477 | //t_start = t.read_us(); |
Skykon | 3:c18342e4fddd | 478 | |
taknokolat | 7:8989a4b84695 | 479 | float rpy[3] = {0}; |
Skykon | 3:c18342e4fddd | 480 | static uint16_t count_changeRPY = 0; |
Skykon | 3:c18342e4fddd | 481 | static bool flg_checkoutlier = false; |
Skykon | 3:c18342e4fddd | 482 | NVIC_DisableIRQ(USART1_IRQn); |
Skykon | 3:c18342e4fddd | 483 | NVIC_DisableIRQ(USART2_IRQn); |
Skykon | 3:c18342e4fddd | 484 | NVIC_DisableIRQ(TIM5_IRQn); |
Skykon | 3:c18342e4fddd | 485 | NVIC_DisableIRQ(EXTI0_IRQn); |
Skykon | 3:c18342e4fddd | 486 | NVIC_DisableIRQ(EXTI9_5_IRQn); |
Skykon | 3:c18342e4fddd | 487 | |
Skykon | 3:c18342e4fddd | 488 | mpu6050.getRollPitchYaw_Skipper(rpy); |
Skykon | 3:c18342e4fddd | 489 | |
Skykon | 3:c18342e4fddd | 490 | NVIC_EnableIRQ(USART1_IRQn); |
Skykon | 3:c18342e4fddd | 491 | NVIC_EnableIRQ(USART2_IRQn); |
Skykon | 3:c18342e4fddd | 492 | NVIC_EnableIRQ(TIM5_IRQn); |
Skykon | 3:c18342e4fddd | 493 | NVIC_EnableIRQ(EXTI0_IRQn); |
Skykon | 3:c18342e4fddd | 494 | NVIC_EnableIRQ(EXTI9_5_IRQn); |
Skykon | 3:c18342e4fddd | 495 | |
Skykon | 3:c18342e4fddd | 496 | |
Skykon | 3:c18342e4fddd | 497 | //外れ値対策 |
Skykon | 3:c18342e4fddd | 498 | for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI; |
Skykon | 3:c18342e4fddd | 499 | rpy[ROLL] -= FirstROLL; |
Skykon | 3:c18342e4fddd | 500 | rpy[PITCH] -= FirstPITCH; |
taknokolat | 4:67f705d42f1e | 501 | if(!setupFlag){ |
taknokolat | 4:67f705d42f1e | 502 | rpy[YAW] -= FirstYAW; |
taknokolat | 4:67f705d42f1e | 503 | }else{ |
taknokolat | 4:67f705d42f1e | 504 | if(rpy[YAW] >= FirstYAW){ |
taknokolat | 4:67f705d42f1e | 505 | rpy[YAW] -= FirstYAW; |
taknokolat | 4:67f705d42f1e | 506 | }else{ |
taknokolat | 4:67f705d42f1e | 507 | rpy[YAW] += 360.0f; |
taknokolat | 4:67f705d42f1e | 508 | rpy[YAW] -= FirstYAW; |
taknokolat | 4:67f705d42f1e | 509 | } |
taknokolat | 4:67f705d42f1e | 510 | } |
Skykon | 3:c18342e4fddd | 511 | |
Skykon | 3:c18342e4fddd | 512 | 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 | 513 | if(!flg_checkoutlier || count_changeRPY >= 2){ |
Skykon | 3:c18342e4fddd | 514 | for(uint8_t i=0; i<3; i++){ |
Skykon | 3:c18342e4fddd | 515 | nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f; //2つの移動平均 |
Skykon | 3:c18342e4fddd | 516 | } |
Skykon | 3:c18342e4fddd | 517 | count_changeRPY = 0; |
Skykon | 3:c18342e4fddd | 518 | }else count_changeRPY++; |
Skykon | 3:c18342e4fddd | 519 | flg_checkoutlier = false; |
Skykon | 3:c18342e4fddd | 520 | |
Skykon | 3:c18342e4fddd | 521 | } |
Skykon | 3:c18342e4fddd | 522 | |
Skykon | 3:c18342e4fddd | 523 | |
Skykon | 3:c18342e4fddd | 524 | void Init_sensors(){ |
Skykon | 3:c18342e4fddd | 525 | if(mpu6050.setup() == -1){ |
Skykon | 3:c18342e4fddd | 526 | pc.printf("failed initialize\r\n"); |
Skykon | 3:c18342e4fddd | 527 | } |
Skykon | 3:c18342e4fddd | 528 | } |
Skykon | 3:c18342e4fddd | 529 | |
Skykon | 3:c18342e4fddd | 530 | |
Skykon | 3:c18342e4fddd | 531 | void DisplayClock(){ |
Skykon | 3:c18342e4fddd | 532 | pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000); |
Skykon | 3:c18342e4fddd | 533 | pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000); |
Skykon | 3:c18342e4fddd | 534 | pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000); |
Skykon | 3:c18342e4fddd | 535 | pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000); |
Skykon | 3:c18342e4fddd | 536 | pc.printf("\r\n"); |
taknokolat | 4:67f705d42f1e | 537 | } |
taknokolat | 6:166746820555 | 538 | |
taknokolat | 6:166746820555 | 539 | void SensingHMC(){ |
taknokolat | 6:166746820555 | 540 | //static int16_t deltaT = 0, t_start = 0; |
taknokolat | 6:166746820555 | 541 | //t_start = t.read_us(); |
taknokolat | 6:166746820555 | 542 | |
taknokolat | 7:8989a4b84695 | 543 | float rpy=0; |
taknokolat | 6:166746820555 | 544 | static uint16_t count_changeRPY = 0; |
taknokolat | 6:166746820555 | 545 | static bool flg_checkoutlier = false; |
taknokolat | 6:166746820555 | 546 | NVIC_DisableIRQ(USART1_IRQn); |
taknokolat | 6:166746820555 | 547 | NVIC_DisableIRQ(USART2_IRQn); |
taknokolat | 6:166746820555 | 548 | NVIC_DisableIRQ(TIM5_IRQn); |
taknokolat | 6:166746820555 | 549 | NVIC_DisableIRQ(EXTI0_IRQn); |
taknokolat | 6:166746820555 | 550 | NVIC_DisableIRQ(EXTI9_5_IRQn); |
taknokolat | 4:67f705d42f1e | 551 | |
taknokolat | 6:166746820555 | 552 | rpy= compass.getHeadingXYDeg(20,50); |
taknokolat | 6:166746820555 | 553 | |
taknokolat | 6:166746820555 | 554 | NVIC_EnableIRQ(USART1_IRQn); |
taknokolat | 6:166746820555 | 555 | NVIC_EnableIRQ(USART2_IRQn); |
taknokolat | 6:166746820555 | 556 | NVIC_EnableIRQ(TIM5_IRQn); |
taknokolat | 6:166746820555 | 557 | NVIC_EnableIRQ(EXTI0_IRQn); |
taknokolat | 6:166746820555 | 558 | NVIC_EnableIRQ(EXTI9_5_IRQn); |
taknokolat | 6:166746820555 | 559 | |
taknokolat | 6:166746820555 | 560 | |
taknokolat | 6:166746820555 | 561 | //外れ値対策 |
taknokolat | 6:166746820555 | 562 | //rpy*= 180.0f/PI; |
taknokolat | 6:166746820555 | 563 | if(!setupFlag){ |
taknokolat | 6:166746820555 | 564 | rpy -= g_FirstYAW_HMC; |
taknokolat | 6:166746820555 | 565 | }else{ |
taknokolat | 6:166746820555 | 566 | if(rpy >= g_FirstYAW_HMC){ |
taknokolat | 6:166746820555 | 567 | rpy -= g_FirstYAW_HMC; |
taknokolat | 6:166746820555 | 568 | }else{ |
taknokolat | 6:166746820555 | 569 | rpy += 360.0f; |
taknokolat | 6:166746820555 | 570 | rpy -= g_FirstYAW_HMC; |
taknokolat | 6:166746820555 | 571 | } |
taknokolat | 6:166746820555 | 572 | } |
taknokolat | 6:166746820555 | 573 | |
taknokolat | 6:166746820555 | 574 | if(rpy < nowAngle_HMC-10 || rpy > nowAngle_HMC+10) {flg_checkoutlier = true;} |
taknokolat | 6:166746820555 | 575 | if(!flg_checkoutlier || count_changeRPY >= 2){ |
taknokolat | 6:166746820555 | 576 | |
taknokolat | 6:166746820555 | 577 | nowAngle_HMC = (rpy + nowAngle_HMC)/2.0f; //2つの移動平均 |
taknokolat | 6:166746820555 | 578 | |
taknokolat | 6:166746820555 | 579 | count_changeRPY = 0; |
taknokolat | 6:166746820555 | 580 | }else count_changeRPY++; |
taknokolat | 6:166746820555 | 581 | flg_checkoutlier = false; |
taknokolat | 6:166746820555 | 582 | |
taknokolat | 6:166746820555 | 583 | } |
taknokolat | 12:083662bca47d | 584 | |
taknokolat | 12:083662bca47d | 585 | int SetOptions(int *Servo_NEUTRAL_R, int *Servo_NEUTRAL_L, |
taknokolat | 12:083662bca47d | 586 | int *Servo_high_FORWARD_R, int *Servo_high_FORWARD_L, |
taknokolat | 12:083662bca47d | 587 | int *Servo_slow_FORWARD_R,int *Servo_slow_FORWARD_L, |
taknokolat | 12:083662bca47d | 588 | int *Turntable_NEUTRAL,int *Matchspeed,int *Focus_NEUTRAL, |
taknokolat | 16:26cee2aaf61d | 589 | int *Camera_deg_A, int *Camera_deg_B, |
taknokolat | 16:26cee2aaf61d | 590 | int *Pint_speed, float *Pint_wait |
taknokolat | 12:083662bca47d | 591 | ){ |
taknokolat | 12:083662bca47d | 592 | |
taknokolat | 12:083662bca47d | 593 | pc.printf("SDsetup start.\r\n"); |
taknokolat | 12:083662bca47d | 594 | |
taknokolat | 12:083662bca47d | 595 | FILE *fp; |
taknokolat | 12:083662bca47d | 596 | char parameter[20]; //文字列渡す用の配列 |
taknokolat | 12:083662bca47d | 597 | int SDerrorcount = 0; //取得できなかった数を返す |
taknokolat | 12:083662bca47d | 598 | const char *paramNames[] = { |
taknokolat | 12:083662bca47d | 599 | "SERVO_NEUTRAL_R", |
taknokolat | 12:083662bca47d | 600 | "SERVO_NEUTRAL_L", |
taknokolat | 12:083662bca47d | 601 | "SERVO_HIGH_FORWARD_R", |
taknokolat | 12:083662bca47d | 602 | "SERVO_HIGH_FORWARD_L", |
taknokolat | 12:083662bca47d | 603 | "SERVO_SLOW_FORWARD_R", |
taknokolat | 12:083662bca47d | 604 | "SERVO_SLOW_FORWARD_L", |
taknokolat | 12:083662bca47d | 605 | "TURNTABLE_NEUTRAL", |
taknokolat | 12:083662bca47d | 606 | "MATCH_SPEED", |
taknokolat | 12:083662bca47d | 607 | "FOCUS_NEUTRAL", |
taknokolat | 12:083662bca47d | 608 | "CAMERA_DEG_A", |
taknokolat | 16:26cee2aaf61d | 609 | "CAMERA_DEG_B", |
taknokolat | 16:26cee2aaf61d | 610 | "PINT_SPEED", |
taknokolat | 16:26cee2aaf61d | 611 | "PINT_WAIT" |
taknokolat | 12:083662bca47d | 612 | }; |
taknokolat | 12:083662bca47d | 613 | |
taknokolat | 12:083662bca47d | 614 | fp = fopen("/sd/option.txt","r"); |
taknokolat | 12:083662bca47d | 615 | |
taknokolat | 12:083662bca47d | 616 | if(fp != NULL){ //開けたら |
taknokolat | 12:083662bca47d | 617 | pc.printf("File was openned.\r\n"); |
taknokolat | 12:083662bca47d | 618 | if(GetParameter(fp,paramNames[0],parameter)) *Servo_NEUTRAL_R = atof(parameter); |
taknokolat | 12:083662bca47d | 619 | else{ *Servo_NEUTRAL_R = servo_NEUTRAL_R; |
taknokolat | 12:083662bca47d | 620 | SDerrorcount++; |
taknokolat | 12:083662bca47d | 621 | } |
taknokolat | 12:083662bca47d | 622 | if(GetParameter(fp,paramNames[1],parameter)) *Servo_NEUTRAL_L = atof(parameter); |
taknokolat | 12:083662bca47d | 623 | else{ *Servo_NEUTRAL_L = servo_NEUTRAL_L; |
taknokolat | 12:083662bca47d | 624 | SDerrorcount++; |
taknokolat | 12:083662bca47d | 625 | } |
taknokolat | 12:083662bca47d | 626 | if(GetParameter(fp,paramNames[2],parameter)) *Servo_high_FORWARD_R = atof(parameter); |
taknokolat | 12:083662bca47d | 627 | else{ *Servo_high_FORWARD_R = servo_high_FORWARD_R; |
taknokolat | 12:083662bca47d | 628 | SDerrorcount++; |
taknokolat | 12:083662bca47d | 629 | } |
taknokolat | 12:083662bca47d | 630 | if(GetParameter(fp,paramNames[3],parameter)) *Servo_high_FORWARD_L = atof(parameter); |
taknokolat | 12:083662bca47d | 631 | else{ *Servo_high_FORWARD_L = servo_high_FORWARD_L; |
taknokolat | 12:083662bca47d | 632 | SDerrorcount++; |
taknokolat | 12:083662bca47d | 633 | } |
taknokolat | 12:083662bca47d | 634 | if(GetParameter(fp,paramNames[4],parameter)) *Servo_slow_FORWARD_R = atof(parameter); |
taknokolat | 12:083662bca47d | 635 | else{ *Servo_slow_FORWARD_R = servo_slow_FORWARD_R; |
taknokolat | 12:083662bca47d | 636 | SDerrorcount++; |
taknokolat | 12:083662bca47d | 637 | } |
taknokolat | 12:083662bca47d | 638 | if(GetParameter(fp,paramNames[5],parameter)) *Servo_slow_FORWARD_L = atof(parameter); |
taknokolat | 12:083662bca47d | 639 | else{ *Servo_slow_FORWARD_L = servo_slow_FORWARD_L; |
taknokolat | 12:083662bca47d | 640 | SDerrorcount++; |
taknokolat | 12:083662bca47d | 641 | } |
taknokolat | 12:083662bca47d | 642 | |
taknokolat | 12:083662bca47d | 643 | if(GetParameter(fp,paramNames[6],parameter)) *Turntable_NEUTRAL = atof(parameter); |
taknokolat | 12:083662bca47d | 644 | else{ *Turntable_NEUTRAL = turntable_NEUTRAL; |
taknokolat | 12:083662bca47d | 645 | SDerrorcount++; |
taknokolat | 12:083662bca47d | 646 | } |
taknokolat | 12:083662bca47d | 647 | if(GetParameter(fp,paramNames[7],parameter)) *Matchspeed = atof(parameter); |
taknokolat | 12:083662bca47d | 648 | else{ *Matchspeed = matchspeed; |
taknokolat | 12:083662bca47d | 649 | SDerrorcount++; |
taknokolat | 12:083662bca47d | 650 | } |
taknokolat | 12:083662bca47d | 651 | if(GetParameter(fp,paramNames[8],parameter)) *Focus_NEUTRAL = atof(parameter); |
taknokolat | 12:083662bca47d | 652 | else{ *Focus_NEUTRAL = focus_NEUTRAL; |
taknokolat | 12:083662bca47d | 653 | SDerrorcount++; |
taknokolat | 12:083662bca47d | 654 | } |
taknokolat | 12:083662bca47d | 655 | if(GetParameter(fp,paramNames[9],parameter)) *Camera_deg_A = atof(parameter); |
taknokolat | 12:083662bca47d | 656 | else{ *Camera_deg_A = camera_deg_A; |
taknokolat | 12:083662bca47d | 657 | SDerrorcount++; |
taknokolat | 12:083662bca47d | 658 | } |
taknokolat | 12:083662bca47d | 659 | if(GetParameter(fp,paramNames[10],parameter)) *Camera_deg_B = atof(parameter); |
taknokolat | 12:083662bca47d | 660 | else{ *Camera_deg_B = camera_deg_B; |
taknokolat | 12:083662bca47d | 661 | SDerrorcount++; |
taknokolat | 12:083662bca47d | 662 | } |
taknokolat | 16:26cee2aaf61d | 663 | if(GetParameter(fp,paramNames[11],parameter)) *Pint_speed = atof(parameter); |
taknokolat | 16:26cee2aaf61d | 664 | else{ *Pint_speed = pint_speed; |
taknokolat | 16:26cee2aaf61d | 665 | SDerrorcount++; |
taknokolat | 16:26cee2aaf61d | 666 | } |
taknokolat | 16:26cee2aaf61d | 667 | if(GetParameter(fp,paramNames[12],parameter)) *Pint_wait = atof(parameter); |
taknokolat | 16:26cee2aaf61d | 668 | else{ *Pint_wait = pint_wait; |
taknokolat | 16:26cee2aaf61d | 669 | SDerrorcount++; |
taknokolat | 16:26cee2aaf61d | 670 | } |
taknokolat | 12:083662bca47d | 671 | |
taknokolat | 12:083662bca47d | 672 | fclose(fp); |
taknokolat | 12:083662bca47d | 673 | |
taknokolat | 12:083662bca47d | 674 | }else{ //ファイルがなかったら |
taknokolat | 12:083662bca47d | 675 | pc.printf("fp was null.\r\n"); |
taknokolat | 12:083662bca47d | 676 | |
taknokolat | 12:083662bca47d | 677 | *Servo_NEUTRAL_R = servo_NEUTRAL_R; |
taknokolat | 12:083662bca47d | 678 | *Servo_NEUTRAL_L = servo_NEUTRAL_L; |
taknokolat | 12:083662bca47d | 679 | *Servo_high_FORWARD_R = servo_high_FORWARD_R; |
taknokolat | 12:083662bca47d | 680 | *Servo_high_FORWARD_L = servo_high_FORWARD_L; |
taknokolat | 12:083662bca47d | 681 | *Servo_slow_FORWARD_R = servo_slow_FORWARD_R; |
taknokolat | 12:083662bca47d | 682 | *Servo_slow_FORWARD_L = servo_slow_FORWARD_L; |
taknokolat | 12:083662bca47d | 683 | *Turntable_NEUTRAL = turntable_NEUTRAL; |
taknokolat | 12:083662bca47d | 684 | *Matchspeed = matchspeed; |
taknokolat | 12:083662bca47d | 685 | *Focus_NEUTRAL = focus_NEUTRAL; |
taknokolat | 12:083662bca47d | 686 | *Camera_deg_A = camera_deg_A; |
taknokolat | 12:083662bca47d | 687 | *Camera_deg_B = camera_deg_B; |
taknokolat | 16:26cee2aaf61d | 688 | *Pint_speed = pint_speed; |
taknokolat | 16:26cee2aaf61d | 689 | *Pint_wait = pint_wait; |
taknokolat | 12:083662bca47d | 690 | SDerrorcount = -1; |
taknokolat | 12:083662bca47d | 691 | } |
taknokolat | 12:083662bca47d | 692 | pc.printf("SDsetup finished.\r\n"); |
taknokolat | 12:083662bca47d | 693 | if(SDerrorcount == 0) pc.printf("setting option is success\r\n"); |
taknokolat | 12:083662bca47d | 694 | else if(SDerrorcount == -1) pc.printf("ERROR 1. cannot open option\r\n"); |
taknokolat | 12:083662bca47d | 695 | else if(SDerrorcount > 0) pc.printf("ERROR 2. reading parameter is failed[%d]\r\n",SDerrorcount); |
taknokolat | 12:083662bca47d | 696 | |
taknokolat | 12:083662bca47d | 697 | pc.printf("Servo_NEUTRAL_R = %d, Servo_NEUTRAL_L = %d\r\n", *Servo_NEUTRAL_R, *Servo_NEUTRAL_L); |
taknokolat | 13:b088f0db7158 | 698 | pc.printf("Servo_high_FORWARD_R = %d, Servo_high_FORWARD_L = %d\r\n", *Servo_high_FORWARD_R, *Servo_high_FORWARD_L); |
taknokolat | 13:b088f0db7158 | 699 | pc.printf("Servo_slow_FORWARD_R = %d, Servo_slow_FORWARD_L = %d\r\n", *Servo_slow_FORWARD_R, *Servo_slow_FORWARD_L); |
taknokolat | 13:b088f0db7158 | 700 | pc.printf("Turntable_NEUTRAL = %d, Matchspeed = %d\r\n", *Turntable_NEUTRAL, *Matchspeed); |
taknokolat | 13:b088f0db7158 | 701 | pc.printf("Focus_NEUTRAL = %d\r\n", *Focus_NEUTRAL); |
taknokolat | 13:b088f0db7158 | 702 | pc.printf("Camera_deg_A = %d, Camera_deg_B = %d\r\n", *Camera_deg_A, *Camera_deg_B); |
taknokolat | 16:26cee2aaf61d | 703 | pc.printf("Pint_speed = %d, Pint_wait = %f\r\n", *Pint_speed, *Pint_wait); |
taknokolat | 12:083662bca47d | 704 | |
taknokolat | 12:083662bca47d | 705 | return SDerrorcount; |
taknokolat | 12:083662bca47d | 706 | } |
taknokolat | 12:083662bca47d | 707 | |
taknokolat | 12:083662bca47d | 708 | int GetParameter(FILE *fp, const char *paramName,char parameter[]){ |
taknokolat | 12:083662bca47d | 709 | int i=0, j=0; |
taknokolat | 12:083662bca47d | 710 | int strmax = 200; |
taknokolat | 12:083662bca47d | 711 | char str[strmax]; |
taknokolat | 12:083662bca47d | 712 | |
taknokolat | 12:083662bca47d | 713 | rewind(fp); //ファイル位置を先頭に |
taknokolat | 12:083662bca47d | 714 | while(1){ |
taknokolat | 12:083662bca47d | 715 | if (fgets(str, strmax, fp) == NULL) { |
taknokolat | 12:083662bca47d | 716 | return 0; |
taknokolat | 12:083662bca47d | 717 | } |
taknokolat | 12:083662bca47d | 718 | if (!strncmp(str, paramName, strlen(paramName))) { |
taknokolat | 12:083662bca47d | 719 | while (str[i++] != '=') {} |
taknokolat | 12:083662bca47d | 720 | while (str[i] != '\n') { |
taknokolat | 12:083662bca47d | 721 | parameter[j++] = str[i++]; |
taknokolat | 12:083662bca47d | 722 | } |
taknokolat | 12:083662bca47d | 723 | parameter[j] = '\0'; |
taknokolat | 12:083662bca47d | 724 | return 1; |
taknokolat | 12:083662bca47d | 725 | } |
taknokolat | 12:083662bca47d | 726 | } |
taknokolat | 12:083662bca47d | 727 | } |
taknokolat | 12:083662bca47d | 728 | |
taknokolat | 12:083662bca47d | 729 | |
taknokolat | 6:166746820555 | 730 | |
taknokolat | 4:67f705d42f1e | 731 | void DebugPrint(){ |
taknokolat | 4:67f705d42f1e | 732 | //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); //skipper地磁気センサ_デバック用 |
taknokolat | 7:8989a4b84695 | 733 | //pc.printf("%3.2f\t",nowAngle[2]); |
taknokolat | 7:8989a4b84695 | 734 | //pc.printf("%3.2f\t",nowAngle_HMC); //HMC |
taknokolat | 7:8989a4b84695 | 735 | //pc.printf("\r\n"); |
taknokolat | 6:166746820555 | 736 | } |