航空研究会 / Mbed 2 deprecated hassi_10_usb

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?

UserRevisionLine numberNew 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 }