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