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