imano yuta / Mbed 2 deprecated sahaquiel_raspi_uart_test

skipper_raspi_uart_test

Dependencies:   mbed MPU6050_2 HMC5883L_4 SDFileSystem3

main.cpp@34:0c2b67b31b12, 2019-03-01 (annotated)

Committer:
taknokolat
Date:
Fri Mar 01 11:03:39 2019 +0000
Revision:
34:0c2b67b31b12
Parent:
33:5f77118eacd5
Child:
35:0f89eff001a7
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 12:083662bca47d 4 #include "SDFileSystem.h"
taknokolat 12:083662bca47d 5 #include "SkipperSv2.h"
taknokolat 12:083662bca47d 6 #include "falfalla.h"
Skykon 3:c18342e4fddd 7
Skykon 3:c18342e4fddd 8 //MPU_check用
Skykon 3:c18342e4fddd 9 #define PI 3.14159265358979
HARUKIDELTA 0:84ddd6d354e1 10
HARUKIDELTA 17:83db05cab3d1 11 #define servo_NEUTRAL_R 1614
HARUKIDELTA 17:83db05cab3d1 12 #define servo_NEUTRAL_L 1621
HARUKIDELTA 0:84ddd6d354e1 13 #define servo_slow_FORWARD_R 1560
HARUKIDELTA 0:84ddd6d354e1 14 #define servo_slow_FORWARD_L 1560
taknokolat 12:083662bca47d 15 #define servo_high_FORWARD_R 1860
taknokolat 12:083662bca47d 16 #define servo_high_FORWARD_L 1860
HARUKIDELTA 17:83db05cab3d1 17 #define turntable_NEUTRAL 1180 //カメラ台座のサーボ
HARUKIDELTA 17:83db05cab3d1 18 #define matchspeed 1500 + 100 //カメラと方向を合わせるときの車輪の速度
HARUKIDELTA 17:83db05cab3d1 19 #define focus_NEUTRAL 1455 //焦点合わせ用サーボ
HARUKIDELTA 17:83db05cab3d1 20 #define camera_deg_A 1400 //カメラ角度調整
HARUKIDELTA 17:83db05cab3d1 21 #define camera_deg_B 1800
HARUKIDELTA 17:83db05cab3d1 22 #define pint_speed 25
taknokolat 20:011977a37394 23 #define pint_wait 3
HARUKIDELTA 17:83db05cab3d1 24 #define turntable_speed 1800
taknokolat 24:0ad1725c7849 25 #define time360 110
taknokolat 24:0ad1725c7849 26 #define match_wid 5
taknokolat 20:011977a37394 27 #define camera_board_wait 100
HARUKIDELTA 0:84ddd6d354e1 28
taknokolat 32:ab030832d180 29 #define ReturnCount 2
taknokolat 32:ab030832d180 30
HARUKIDELTA 0:84ddd6d354e1 31 #define MOVE_NEUTRAL 0
taknokolat 19:949bab1e9451 32 #define MOVE_FORWARD 1
HARUKIDELTA 0:84ddd6d354e1 33 #define MOVE_LEFT 2
HARUKIDELTA 0:84ddd6d354e1 34 #define MOVE_RIGHT 3
HARUKIDELTA 0:84ddd6d354e1 35 #define MOVE_BACK 4
Skykon 3:c18342e4fddd 36 #define GOAL_FORWARD 5 //ゴール付近_ゆっくり
HARUKIDELTA 0:84ddd6d354e1 37 #define GOAL_LEFT 6
HARUKIDELTA 0:84ddd6d354e1 38 #define GOAL_RIGHT 7
Skykon 3:c18342e4fddd 39 #define MAX_FORWARD 8 //はやい_姿勢修正用
HARUKIDELTA 0:84ddd6d354e1 40 #define MAX_BACK 9
HARUKIDELTA 0:84ddd6d354e1 41
taknokolat 1:290e621741fd 42 void getSF_Serial_jevois();
taknokolat 1:290e621741fd 43 void getSF_Serial_pi();
taknokolat 1:290e621741fd 44
Skykon 3:c18342e4fddd 45 //MPU_check用
Skykon 3:c18342e4fddd 46 void SensingMPU();
taknokolat 7:8989a4b84695 47
taknokolat 7:8989a4b84695 48 void MoveCansat(char g_landingcommand);
Skykon 3:c18342e4fddd 49 void setup();
Skykon 3:c18342e4fddd 50 void Init_sensors();
Skykon 3:c18342e4fddd 51 void DisplayClock();
taknokolat 4:67f705d42f1e 52 void DebugPrint();
taknokolat 6:166746820555 53 void SensingHMC();
taknokolat 7:8989a4b84695 54 void MoveCameraBoard();
taknokolat 7:8989a4b84695 55 void MatchPosition();
taknokolat 7:8989a4b84695 56 void FocusAdjust();
Skykon 3:c18342e4fddd 57
taknokolat 12:083662bca47d 58 //sd設定
taknokolat 12:083662bca47d 59 int GetParameter(FILE *fp, const char *paramName,char parameter[]);
taknokolat 12:083662bca47d 60
taknokolat 13:b088f0db7158 61 int SetOptions(int *Servo_NEUTRAL_R,int *Servo_NEUTRAL_L,
taknokolat 12:083662bca47d 62 int *Servo_high_FORWARD_R,int *Servo_high_FORWARD_L,
taknokolat 12:083662bca47d 63 int *Servo_slow_FORWARD_R,int *Servo_slow_FORWARD_L,
taknokolat 12:083662bca47d 64 int *Turntable_NEUTRAL,int *Matchspeed,int *Focus_NEUTRAL,
taknokolat 16:26cee2aaf61d 65 int *Camera_deg_A, int *Camera_deg_B,
HARUKIDELTA 17:83db05cab3d1 66 int *Pint_speed, float *Pint_wait,
taknokolat 19:949bab1e9451 67 int *Turntable_speed,
taknokolat 24:0ad1725c7849 68 int *Time360, int *Match_wid,
taknokolat 20:011977a37394 69 int *Camera_board_wait
taknokolat 12:083662bca47d 70 );
taknokolat 12:083662bca47d 71
taknokolat 6:166746820555 72 static float nowAngle[3] = {0,0,0},nowAngle_HMC=0;
taknokolat 6:166746820555 73 float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0,g_FirstYAW_HMC;
Skykon 3:c18342e4fddd 74
taknokolat 7:8989a4b84695 75 bool setupFlag = false;
taknokolat 7:8989a4b84695 76 bool CameraDegFlag = false;
taknokolat 30:90252dc48c1a 77 bool jevoisFlag = true;
taknokolat 31:210cc32d3175 78 bool FocusFlag = false;
taknokolat 4:67f705d42f1e 79
taknokolat 24:0ad1725c7849 80 int g_CameraDegCounter = 0; //カメラの回転数をカウント
taknokolat 24:0ad1725c7849 81
Skykon 3:c18342e4fddd 82 enum Angle{ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度
Skykon 3:c18342e4fddd 83
Skykon 3:c18342e4fddd 84 Timer t;
HARUKIDELTA 26:ea86e69563b5 85 Timer t2;
Skykon 3:c18342e4fddd 86
HARUKIDELTA 25:1f938342d5f9 87 FILE *g_fp;
HARUKIDELTA 25:1f938342d5f9 88
taknokolat 14:90e9b44eb819 89 SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd");
taknokolat 14:90e9b44eb819 90
taknokolat 30:90252dc48c1a 91 RawSerial pc(PA_2,PA_3,115200); //uart2 jevoiis
taknokolat 11:d44d137831b9 92 //pa2:UART2_TX,pa3:UART2_RX
taknokolat 30:90252dc48c1a 93 RawSerial pc2(PA_11,PA_12,115200); //uart1 raspberry
taknokolat 11:d44d137831b9 94 //pb6:UART1_TX,pb7:UART1_RX
taknokolat 11:d44d137831b9 95
HARUKIDELTA 5:8bfe95431ec0 96 //PWM pin宣言
taknokolat 6:166746820555 97 PwmOut servoR(PC_6); //TIM3_CH1 車輪右
taknokolat 6:166746820555 98 PwmOut servoL(PC_7); //TIM3_CH2 車輪左
HARUKIDELTA 5:8bfe95431ec0 99 PwmOut servoTurnTable(PB_0); //TIM3_CH3 カメラ台回転Servo
HARUKIDELTA 5:8bfe95431ec0 100 PwmOut servoCameradeg(PB_1); //TIM3_CH4 カメラ角度調節Servo
HARUKIDELTA 5:8bfe95431ec0 101 PwmOut servoCameraPinto(PB_6); //TIM4_CH1 カメラピント合わせ
HARUKIDELTA 5:8bfe95431ec0 102 PwmOut servoCameramount(PA_6); //skipperウラ カメラマウント起動
HARUKIDELTA 5:8bfe95431ec0 103 PwmOut servoGetUP(PC_8); //skipperウラ 起き上がり動作
Skykon 3:c18342e4fddd 104
HARUKIDELTA 5:8bfe95431ec0 105 /*通信用のpinは
HARUKIDELTA 5:8bfe95431ec0 106 PA_3(UART2_Rx) skipperウラ
HARUKIDELTA 5:8bfe95431ec0 107 PA_12(UART6_Rx) skipperオモテ USB端子より
HARUKIDELTA 5:8bfe95431ec0 108 PB_7 (UART1_Rx) skipperオモテ 6番目 TIM4_CH2
HARUKIDELTA 5:8bfe95431ec0 109 */
HARUKIDELTA 5:8bfe95431ec0 110
HARUKIDELTA 5:8bfe95431ec0 111 /*超音波はRaspberryPiに積む*/
HARUKIDELTA 5:8bfe95431ec0 112
HARUKIDELTA 23:29b2722bd753 113 DigitalOut led1(PA_0); //tanakaOK kimuraは動作不良
HARUKIDELTA 23:29b2722bd753 114 DigitalOut led2(PA_1); //tanakaOK kimuraOK
HARUKIDELTA 23:29b2722bd753 115 DigitalOut led3(PB_5); //使ってないよ
HARUKIDELTA 23:29b2722bd753 116 DigitalOut led4(PB_4); //使ってないよ
HARUKIDELTA 17:83db05cab3d1 117
HARUKIDELTA 17:83db05cab3d1 118
HARUKIDELTA 5:8bfe95431ec0 119 //外付けコンパス
taknokolat 24:0ad1725c7849 120 //HMC5883L compass(PB_9, PB_8); //コンパスセンサー TIM4_CH3とCH4
HARUKIDELTA 0:84ddd6d354e1 121
taknokolat 2:f30666d7838b 122 char g_landingcommand='N';
taknokolat 1:290e621741fd 123
Skykon 3:c18342e4fddd 124 //MPU_check用
Skykon 3:c18342e4fddd 125 MPU6050DMP6 mpu6050(PC_0,&pc);
Skykon 3:c18342e4fddd 126
Skykon 3:c18342e4fddd 127
HARUKIDELTA 0:84ddd6d354e1 128 int main() {
taknokolat 1:290e621741fd 129
Skykon 3:c18342e4fddd 130 //MPU_check
Skykon 3:c18342e4fddd 131 setup();
taknokolat 19:949bab1e9451 132 //コンパスチェック用
taknokolat 19:949bab1e9451 133 /*while(1){
taknokolat 22:a19be3b505b7 134 //SensingMPU();
taknokolat 19:949bab1e9451 135 SensingHMC();
taknokolat 22:a19be3b505b7 136 //pc.printf("%3.2f\t",nowAngle[2]);
taknokolat 19:949bab1e9451 137 pc.printf("%3.2f\t",nowAngle_HMC); //HMC
taknokolat 19:949bab1e9451 138 pc.printf("\r\n");
taknokolat 19:949bab1e9451 139 wait_ms(30);
taknokolat 19:949bab1e9451 140 }*/
Skykon 3:c18342e4fddd 141
taknokolat 11:d44d137831b9 142
taknokolat 1:290e621741fd 143 // シリアル通信受信の割り込みイベント登録
taknokolat 1:290e621741fd 144 pc.attach(getSF_Serial_jevois, Serial::RxIrq);
taknokolat 12:083662bca47d 145 pc2.attach(getSF_Serial_pi, Serial::RxIrq);
HARUKIDELTA 27:f67efcee6509 146 t2.start();
taknokolat 1:290e621741fd 147
taknokolat 30:90252dc48c1a 148 /*NVIC_DisableIRQ(USART2_IRQn);
taknokolat 29:5d239812ace6 149 while(g_landingcommand != 'B'){
taknokolat 29:5d239812ace6 150 wait_ms(30);
taknokolat 29:5d239812ace6 151 }
taknokolat 29:5d239812ace6 152
taknokolat 29:5d239812ace6 153 jevoisFlag = true;
taknokolat 29:5d239812ace6 154 NVIC_EnableIRQ(USART2_IRQn);
taknokolat 29:5d239812ace6 155 wait(5);
taknokolat 29:5d239812ace6 156 pc.printf("start\r\n");
taknokolat 30:90252dc48c1a 157 */
taknokolat 31:210cc32d3175 158
taknokolat 31:210cc32d3175 159 servoCameraPinto.pulsewidth_us(Focus_NEUTRAL-200);
taknokolat 31:210cc32d3175 160 wait(1);
taknokolat 31:210cc32d3175 161 servoCameraPinto.pulsewidth_us(Focus_NEUTRAL);
taknokolat 31:210cc32d3175 162
HARUKIDELTA 0:84ddd6d354e1 163 while(1) {
taknokolat 30:90252dc48c1a 164 if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
taknokolat 7:8989a4b84695 165 else NVIC_DisableIRQ(USART2_IRQn);
taknokolat 20:011977a37394 166 pc.printf("Move Camera Board\r\n");
taknokolat 10:63fe920595a7 167 MoveCameraBoard();
taknokolat 10:63fe920595a7 168
taknokolat 20:011977a37394 169 pc.printf("Position\r\n");
taknokolat 22:a19be3b505b7 170 pc.printf("g_landingcommand=%c\r\n",g_landingcommand);
HARUKIDELTA 25:1f938342d5f9 171
taknokolat 30:90252dc48c1a 172 NVIC_DisableIRQ(USART6_IRQn);
taknokolat 22:a19be3b505b7 173 NVIC_DisableIRQ(USART2_IRQn);
taknokolat 7:8989a4b84695 174 if(g_landingcommand!='N') MatchPosition();
taknokolat 30:90252dc48c1a 175 NVIC_EnableIRQ(USART6_IRQn);
taknokolat 22:a19be3b505b7 176 NVIC_EnableIRQ(USART2_IRQn);
taknokolat 8:d11a59d2a2f1 177
taknokolat 30:90252dc48c1a 178 if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
taknokolat 8:d11a59d2a2f1 179 else NVIC_DisableIRQ(USART2_IRQn);
taknokolat 8:d11a59d2a2f1 180
taknokolat 20:011977a37394 181 pc.printf("Move Cansat\r\n");
taknokolat 7:8989a4b84695 182 MoveCansat(g_landingcommand);
taknokolat 30:90252dc48c1a 183 if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
taknokolat 22:a19be3b505b7 184 else NVIC_DisableIRQ(USART2_IRQn);
Skykon 3:c18342e4fddd 185 wait_ms(23);
taknokolat 8:d11a59d2a2f1 186
taknokolat 20:011977a37394 187 pc.printf("finish\r\n");
taknokolat 30:90252dc48c1a 188 if(jevoisFlag==true) NVIC_EnableIRQ(USART6_IRQn);
taknokolat 7:8989a4b84695 189 else NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 190 }
HARUKIDELTA 0:84ddd6d354e1 191 }
HARUKIDELTA 0:84ddd6d354e1 192
Skykon 3:c18342e4fddd 193
taknokolat 22:a19be3b505b7 194 void MoveCansat(char a)
HARUKIDELTA 0:84ddd6d354e1 195 {
taknokolat 30:90252dc48c1a 196 NVIC_DisableIRQ(USART6_IRQn);
taknokolat 22:a19be3b505b7 197 NVIC_DisableIRQ(USART2_IRQn);
taknokolat 22:a19be3b505b7 198 switch(a){
taknokolat 2:f30666d7838b 199 case 'N': //MOVE_NEUTRAL
HARUKIDELTA 17:83db05cab3d1 200 servoR.pulsewidth_us(Servo_NEUTRAL_R);
HARUKIDELTA 17:83db05cab3d1 201 servoL.pulsewidth_us(Servo_NEUTRAL_L);
taknokolat 30:90252dc48c1a 202 NVIC_EnableIRQ(USART6_IRQn);
taknokolat 22:a19be3b505b7 203 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 26:ea86e69563b5 204 g_fp = fopen( "/sd/Datalog01.txt" ,"a" );
HARUKIDELTA 27:f67efcee6509 205 fprintf(g_fp, "Time = %d min %d sec : Goal is not found.\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 26:ea86e69563b5 206 fclose(g_fp);
HARUKIDELTA 0:84ddd6d354e1 207 break;
taknokolat 1:290e621741fd 208
taknokolat 2:f30666d7838b 209 case 'Y': //MOVE_FORWARD
HARUKIDELTA 17:83db05cab3d1 210 servoR.pulsewidth_us(Servo_high_FORWARD_R);
HARUKIDELTA 17:83db05cab3d1 211 servoL.pulsewidth_us(Servo_high_FORWARD_L);
taknokolat 30:90252dc48c1a 212 NVIC_EnableIRQ(USART6_IRQn);
taknokolat 22:a19be3b505b7 213 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 27:f67efcee6509 214 g_fp = fopen( "/sd/Datalog01.txt" ,"a" );
HARUKIDELTA 27:f67efcee6509 215 fprintf(g_fp, "Time = %d min %d sec : Cansat move forward .\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 27:f67efcee6509 216 fclose(g_fp);
HARUKIDELTA 0:84ddd6d354e1 217 break;
taknokolat 1:290e621741fd 218
taknokolat 12:083662bca47d 219 case 'l': //MOVE_LEFT Low Speed
HARUKIDELTA 17:83db05cab3d1 220 servoR.pulsewidth_us(Servo_slow_FORWARD_R);
taknokolat 22:a19be3b505b7 221 servoL.pulsewidth_us(Servo_NEUTRAL_L);
taknokolat 30:90252dc48c1a 222 NVIC_EnableIRQ(USART6_IRQn);
taknokolat 22:a19be3b505b7 223 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 27:f67efcee6509 224 g_fp = fopen( "/sd/Datalog01.txt","a" );
HARUKIDELTA 27:f67efcee6509 225 fprintf(g_fp, "Time = %d min %d sec : Cansat move left low speed.\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 27:f67efcee6509 226 fclose(g_fp);
HARUKIDELTA 27:f67efcee6509 227 break;
HARUKIDELTA 27:f67efcee6509 228
taknokolat 12:083662bca47d 229 case 'L': //MOVE_LEFT High Speed
HARUKIDELTA 17:83db05cab3d1 230 servoR.pulsewidth_us(Servo_high_FORWARD_R);
taknokolat 22:a19be3b505b7 231 servoL.pulsewidth_us(Servo_NEUTRAL_L);
taknokolat 30:90252dc48c1a 232 NVIC_EnableIRQ(USART6_IRQn);
taknokolat 22:a19be3b505b7 233 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 27:f67efcee6509 234 g_fp = fopen( "/sd/Datalog01.txt","a" );
HARUKIDELTA 27:f67efcee6509 235 fprintf(g_fp, "Time = %d min %d sec : Cansat move left high speed.\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 27:f67efcee6509 236 fclose(g_fp);
HARUKIDELTA 27:f67efcee6509 237 break;
taknokolat 1:290e621741fd 238
taknokolat 12:083662bca47d 239 case 'r': //MOVE_RIGHT Low Speed
taknokolat 22:a19be3b505b7 240 servoR.pulsewidth_us(Servo_NEUTRAL_R);
HARUKIDELTA 17:83db05cab3d1 241 servoL.pulsewidth_us(Servo_slow_FORWARD_L);
taknokolat 30:90252dc48c1a 242 NVIC_EnableIRQ(USART6_IRQn);
taknokolat 22:a19be3b505b7 243 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 27:f67efcee6509 244 g_fp = fopen( "/sd/Datalog01.txt","a" );
HARUKIDELTA 27:f67efcee6509 245 fprintf(g_fp, "Time = %d min %d sec : Cansat move right low speed.\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 27:f67efcee6509 246 fclose(g_fp);
HARUKIDELTA 0:84ddd6d354e1 247 break;
taknokolat 12:083662bca47d 248
taknokolat 12:083662bca47d 249 case 'R': //MOVE_RIGHT High Speed
taknokolat 22:a19be3b505b7 250 servoR.pulsewidth_us(Servo_NEUTRAL_R);
HARUKIDELTA 17:83db05cab3d1 251 servoL.pulsewidth_us(Servo_high_FORWARD_L);
taknokolat 30:90252dc48c1a 252 NVIC_EnableIRQ(USART6_IRQn);
taknokolat 22:a19be3b505b7 253 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 27:f67efcee6509 254 g_fp = fopen( "/sd/Datalog01.txt","a" );
HARUKIDELTA 27:f67efcee6509 255 fprintf(g_fp, "Time = %d min %d sec : Cansat move left high speed.\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 27:f67efcee6509 256 fclose(g_fp);
HARUKIDELTA 0:84ddd6d354e1 257 break;
taknokolat 1:290e621741fd 258
taknokolat 1:290e621741fd 259 case 'G': //GOAL_FORWARD
HARUKIDELTA 17:83db05cab3d1 260 servoR.pulsewidth_us(Servo_slow_FORWARD_R);
HARUKIDELTA 17:83db05cab3d1 261 servoL.pulsewidth_us(Servo_slow_FORWARD_L);
taknokolat 30:90252dc48c1a 262 NVIC_EnableIRQ(USART6_IRQn);
taknokolat 22:a19be3b505b7 263 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 27:f67efcee6509 264 g_fp = fopen( "/sd/Datalog01.txt","a" );
HARUKIDELTA 27:f67efcee6509 265 fprintf(g_fp, "Time = %d min %d sec : Cansat move goal forward mode.\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 27:f67efcee6509 266 fclose(g_fp);
taknokolat 7:8989a4b84695 267 break;
taknokolat 12:083662bca47d 268
taknokolat 12:083662bca47d 269 case '1': //MOVE_FORWARD Speed Level 1
HARUKIDELTA 17:83db05cab3d1 270 servoR.pulsewidth_us(Servo_high_FORWARD_R);
HARUKIDELTA 17:83db05cab3d1 271 servoL.pulsewidth_us(Servo_high_FORWARD_L);
taknokolat 12:083662bca47d 272 wait(1);
taknokolat 12:083662bca47d 273 do{
taknokolat 22:a19be3b505b7 274 SensingMPU();
taknokolat 22:a19be3b505b7 275 wait_ms(30);
taknokolat 22:a19be3b505b7 276 }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
taknokolat 30:90252dc48c1a 277 NVIC_EnableIRQ(USART6_IRQn);
taknokolat 22:a19be3b505b7 278 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 27:f67efcee6509 279 g_fp = fopen( "/sd/Datalog01.txt","a" );
HARUKIDELTA 27:f67efcee6509 280 fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 1sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 27:f67efcee6509 281 fclose(g_fp);
taknokolat 12:083662bca47d 282 break;
taknokolat 12:083662bca47d 283
taknokolat 12:083662bca47d 284 case '2': //MOVE_FORWARD Speed Level 2
HARUKIDELTA 17:83db05cab3d1 285 servoR.pulsewidth_us(Servo_high_FORWARD_R);
HARUKIDELTA 17:83db05cab3d1 286 servoL.pulsewidth_us(Servo_high_FORWARD_L);
taknokolat 12:083662bca47d 287 wait(2);
taknokolat 12:083662bca47d 288 do{
taknokolat 22:a19be3b505b7 289 SensingMPU();
taknokolat 22:a19be3b505b7 290 wait_ms(30);
taknokolat 22:a19be3b505b7 291 }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
taknokolat 30:90252dc48c1a 292 NVIC_EnableIRQ(USART6_IRQn);
taknokolat 22:a19be3b505b7 293 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 27:f67efcee6509 294 g_fp = fopen( "/sd/Datalog01.txt","a" );
HARUKIDELTA 27:f67efcee6509 295 fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 2sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 27:f67efcee6509 296 fclose(g_fp);
taknokolat 12:083662bca47d 297 break;
taknokolat 12:083662bca47d 298
taknokolat 12:083662bca47d 299 case '3': //MOVE_FORWARD Speed Level 3
HARUKIDELTA 17:83db05cab3d1 300 servoR.pulsewidth_us(Servo_high_FORWARD_R);
HARUKIDELTA 17:83db05cab3d1 301 servoL.pulsewidth_us(Servo_high_FORWARD_L);
taknokolat 12:083662bca47d 302 wait(3);
taknokolat 12:083662bca47d 303 do{
taknokolat 22:a19be3b505b7 304 SensingMPU();
taknokolat 22:a19be3b505b7 305 wait_ms(30);
taknokolat 22:a19be3b505b7 306 }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
taknokolat 30:90252dc48c1a 307 NVIC_EnableIRQ(USART6_IRQn);
taknokolat 22:a19be3b505b7 308 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 27:f67efcee6509 309 g_fp = fopen( "/sd/Datalog01.txt","a" );
HARUKIDELTA 27:f67efcee6509 310 fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 3sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 27:f67efcee6509 311 fclose(g_fp);
taknokolat 12:083662bca47d 312 break;
taknokolat 12:083662bca47d 313
taknokolat 12:083662bca47d 314 case '4': //MOVE_FORWARD Speed Level 4
HARUKIDELTA 17:83db05cab3d1 315 servoR.pulsewidth_us(Servo_high_FORWARD_R);
HARUKIDELTA 17:83db05cab3d1 316 servoL.pulsewidth_us(Servo_high_FORWARD_L);
taknokolat 12:083662bca47d 317 wait(4);
taknokolat 12:083662bca47d 318 do{
taknokolat 22:a19be3b505b7 319 SensingMPU();
taknokolat 22:a19be3b505b7 320 wait_ms(30);
taknokolat 22:a19be3b505b7 321 }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)||(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
taknokolat 30:90252dc48c1a 322 NVIC_EnableIRQ(USART6_IRQn);
taknokolat 22:a19be3b505b7 323 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 27:f67efcee6509 324 g_fp = fopen( "/sd/Datalog01.txt","a" );
HARUKIDELTA 27:f67efcee6509 325 fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 4sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 27:f67efcee6509 326 fclose(g_fp);
taknokolat 12:083662bca47d 327 break;
taknokolat 7:8989a4b84695 328
taknokolat 12:083662bca47d 329 case '5': //MOVE_FORWARD Speed Level 5
HARUKIDELTA 17:83db05cab3d1 330 servoR.pulsewidth_us(Servo_high_FORWARD_R);
HARUKIDELTA 17:83db05cab3d1 331 servoL.pulsewidth_us(Servo_high_FORWARD_L);
taknokolat 7:8989a4b84695 332 wait(5);
taknokolat 10:63fe920595a7 333 do{
taknokolat 22:a19be3b505b7 334 SensingMPU();
taknokolat 22:a19be3b505b7 335 wait_ms(30);
taknokolat 22:a19be3b505b7 336 }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
taknokolat 30:90252dc48c1a 337 NVIC_EnableIRQ(USART6_IRQn);
taknokolat 22:a19be3b505b7 338 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 27:f67efcee6509 339 g_fp = fopen( "/sd/Datalog01.txt","a" );
HARUKIDELTA 27:f67efcee6509 340 fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 5sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 27:f67efcee6509 341 fclose(g_fp);
taknokolat 7:8989a4b84695 342 break;
taknokolat 7:8989a4b84695 343
taknokolat 7:8989a4b84695 344 case 'M': //MatchPosition
HARUKIDELTA 17:83db05cab3d1 345 servoR.pulsewidth_us(Matchspeed);
taknokolat 30:90252dc48c1a 346 NVIC_EnableIRQ(USART6_IRQn);
taknokolat 22:a19be3b505b7 347 NVIC_EnableIRQ(USART2_IRQn);
taknokolat 7:8989a4b84695 348 break;
taknokolat 7:8989a4b84695 349
taknokolat 12:083662bca47d 350 case 'P': //jevoisからraspberry piへの切り替え
taknokolat 7:8989a4b84695 351 jevoisFlag = false;
taknokolat 28:887df143fa9c 352 servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
taknokolat 28:887df143fa9c 353 wait_ms((float)Time360/(float)2);
taknokolat 28:887df143fa9c 354 servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
taknokolat 30:90252dc48c1a 355 NVIC_EnableIRQ(USART6_IRQn);
taknokolat 22:a19be3b505b7 356 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 357 break;
taknokolat 12:083662bca47d 358
taknokolat 28:887df143fa9c 359 case 'B':
taknokolat 12:083662bca47d 360 /*RasPiからの超音波判定(プログラムスタート部)*/
taknokolat 30:90252dc48c1a 361 NVIC_EnableIRQ(USART6_IRQn);
taknokolat 22:a19be3b505b7 362 NVIC_EnableIRQ(USART2_IRQn);
taknokolat 12:083662bca47d 363 break;
taknokolat 1:290e621741fd 364
taknokolat 1:290e621741fd 365 default :
taknokolat 30:90252dc48c1a 366 NVIC_EnableIRQ(USART6_IRQn);
taknokolat 22:a19be3b505b7 367 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 368 break;
taknokolat 1:290e621741fd 369
HARUKIDELTA 0:84ddd6d354e1 370 }
taknokolat 7:8989a4b84695 371
HARUKIDELTA 0:84ddd6d354e1 372 return;
taknokolat 1:290e621741fd 373 }
taknokolat 1:290e621741fd 374
taknokolat 7:8989a4b84695 375 void MoveCameraBoard(){
taknokolat 22:a19be3b505b7 376 //pc.printf("start\r\n");
taknokolat 7:8989a4b84695 377 MoveCansat('N');
taknokolat 22:a19be3b505b7 378 //pc.printf("ok\r\n");
taknokolat 10:63fe920595a7 379 g_landingcommand='N';
taknokolat 29:5d239812ace6 380 servoCameradeg.pulsewidth_us(Camera_deg_B);
taknokolat 29:5d239812ace6 381 wait_ms(30);
taknokolat 32:ab030832d180 382 if(g_CameraDegCounter==0){
taknokolat 32:ab030832d180 383 servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
taknokolat 32:ab030832d180 384 wait_ms(Camera_board_wait*ReturnCount);
taknokolat 32:ab030832d180 385 servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
taknokolat 32:ab030832d180 386
taknokolat 32:ab030832d180 387
taknokolat 32:ab030832d180 388 for(int i=200; i<Camera_deg_B-Camera_deg_A;i=i+200){
taknokolat 32:ab030832d180 389
taknokolat 32:ab030832d180 390 servoCameradeg.pulsewidth_us(Camera_deg_B-i);
taknokolat 32:ab030832d180 391 servoR.pulsewidth_us(Servo_NEUTRAL_R); //servo initialize
taknokolat 32:ab030832d180 392 servoL.pulsewidth_us(Servo_NEUTRAL_L);
taknokolat 32:ab030832d180 393 wait_ms(30);
taknokolat 32:ab030832d180 394 //pc.printf("zoom2\r\n");
taknokolat 32:ab030832d180 395 if(jevoisFlag == true) FocusAdjust();
taknokolat 32:ab030832d180 396 else wait(1);
taknokolat 32:ab030832d180 397 if(g_landingcommand!='N') return;
taknokolat 32:ab030832d180 398
taknokolat 32:ab030832d180 399 }
taknokolat 32:ab030832d180 400
taknokolat 32:ab030832d180 401 }
taknokolat 32:ab030832d180 402
taknokolat 24:0ad1725c7849 403 servoTurnTable.pulsewidth_us(Turntable_NEUTRAL - Turntable_speed);
taknokolat 20:011977a37394 404 wait_ms(Camera_board_wait);
taknokolat 24:0ad1725c7849 405 g_CameraDegCounter++;
HARUKIDELTA 17:83db05cab3d1 406 servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
taknokolat 22:a19be3b505b7 407 //pc.printf("zoom1\r\n");
taknokolat 7:8989a4b84695 408 if(jevoisFlag == true) FocusAdjust();
taknokolat 7:8989a4b84695 409 else wait(1);
taknokolat 32:ab030832d180 410
taknokolat 32:ab030832d180 411
taknokolat 7:8989a4b84695 412 if(g_landingcommand!='N') return;
taknokolat 29:5d239812ace6 413
taknokolat 29:5d239812ace6 414 for(int i=200; i<Camera_deg_B-Camera_deg_A;i=i+200){
taknokolat 29:5d239812ace6 415
taknokolat 32:ab030832d180 416 servoCameradeg.pulsewidth_us(Camera_deg_B-i);
taknokolat 32:ab030832d180 417 servoR.pulsewidth_us(Servo_NEUTRAL_R); //servo initialize
taknokolat 32:ab030832d180 418 servoL.pulsewidth_us(Servo_NEUTRAL_L);
taknokolat 32:ab030832d180 419 wait_ms(30);
taknokolat 29:5d239812ace6 420 //pc.printf("zoom2\r\n");
taknokolat 29:5d239812ace6 421 if(jevoisFlag == true) FocusAdjust();
taknokolat 29:5d239812ace6 422 else wait(1);
taknokolat 29:5d239812ace6 423 if(g_landingcommand!='N') return;
taknokolat 29:5d239812ace6 424
taknokolat 7:8989a4b84695 425 }
taknokolat 7:8989a4b84695 426
taknokolat 20:011977a37394 427
taknokolat 32:ab030832d180 428 pc.printf("g_CameraDegCounter = %d\r\n",g_CameraDegCounter);
taknokolat 22:a19be3b505b7 429 //pc.printf("Move Board Finish\r\n");
taknokolat 7:8989a4b84695 430 return;
taknokolat 7:8989a4b84695 431 }
taknokolat 7:8989a4b84695 432
taknokolat 7:8989a4b84695 433 void MatchPosition(){
taknokolat 32:ab030832d180 434 NVIC_DisableIRQ(USART6_IRQn);
taknokolat 32:ab030832d180 435 NVIC_DisableIRQ(USART2_IRQn);
taknokolat 22:a19be3b505b7 436 pc.printf("MatchPosition\r\n");
taknokolat 24:0ad1725c7849 437
taknokolat 32:ab030832d180 438 int TurnTime ;
taknokolat 32:ab030832d180 439
taknokolat 32:ab030832d180 440 TurnTime = g_CameraDegCounter - ReturnCount;
taknokolat 32:ab030832d180 441
taknokolat 32:ab030832d180 442 if(TurnTime >=0){
taknokolat 32:ab030832d180 443 servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
taknokolat 32:ab030832d180 444 wait_ms(Camera_board_wait*TurnTime);
taknokolat 32:ab030832d180 445 }else{
taknokolat 32:ab030832d180 446 servoTurnTable.pulsewidth_us(Turntable_NEUTRAL - Turntable_speed);
taknokolat 32:ab030832d180 447 wait_ms(-Camera_board_wait*TurnTime);
taknokolat 32:ab030832d180 448 }
taknokolat 24:0ad1725c7849 449 servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
taknokolat 24:0ad1725c7849 450
taknokolat 22:a19be3b505b7 451 int SetLoop=0;
taknokolat 22:a19be3b505b7 452
taknokolat 22:a19be3b505b7 453 while(SetLoop<30){
taknokolat 7:8989a4b84695 454 SensingMPU();
taknokolat 22:a19be3b505b7 455 wait_ms(20);
taknokolat 24:0ad1725c7849 456 //SensingHMC();
taknokolat 24:0ad1725c7849 457 //wait_ms(20);
taknokolat 22:a19be3b505b7 458 //DebugPrint();
taknokolat 22:a19be3b505b7 459 SetLoop++;
taknokolat 22:a19be3b505b7 460 //pc.printf("nowAngle_HMC=%f\tMPU=%f\r\n",nowAngle_HMC,nowAngle[YAW]);
taknokolat 22:a19be3b505b7 461
taknokolat 22:a19be3b505b7 462 }
taknokolat 7:8989a4b84695 463
taknokolat 24:0ad1725c7849 464 static float TargetDeg = 0;
taknokolat 24:0ad1725c7849 465
taknokolat 32:ab030832d180 466 TargetDeg = 360*(float)Camera_board_wait*((float)g_CameraDegCounter - ReturnCount)/(float)Time360;
taknokolat 24:0ad1725c7849 467
taknokolat 33:5f77118eacd5 468 if(TargetDeg > 360)TargetDeg = TargetDeg - 360;
taknokolat 33:5f77118eacd5 469
taknokolat 24:0ad1725c7849 470 float HighTargetYaw = TargetDeg + Match_wid;
taknokolat 24:0ad1725c7849 471 float LowTargetYaw = TargetDeg - Match_wid;
taknokolat 21:ca8f35e24e66 472
taknokolat 22:a19be3b505b7 473
taknokolat 21:ca8f35e24e66 474 if(HighTargetYaw >= 360.0) HighTargetYaw = HighTargetYaw - 360.0;
taknokolat 22:a19be3b505b7 475
taknokolat 21:ca8f35e24e66 476 if(LowTargetYaw < 0) LowTargetYaw = LowTargetYaw + 360.0;
taknokolat 21:ca8f35e24e66 477
taknokolat 22:a19be3b505b7 478 pc.printf("\r\nnow=%f\t,high=%f\t,low=%f\r\n",nowAngle[YAW],HighTargetYaw,LowTargetYaw);
taknokolat 22:a19be3b505b7 479
taknokolat 24:0ad1725c7849 480 MoveCansat('r');
taknokolat 22:a19be3b505b7 481
taknokolat 22:a19be3b505b7 482 if(HighTargetYaw-LowTargetYaw<0){
taknokolat 22:a19be3b505b7 483 while(nowAngle[YAW] > HighTargetYaw && nowAngle[YAW] < LowTargetYaw){
taknokolat 22:a19be3b505b7 484 //MoveCansat('r');
taknokolat 22:a19be3b505b7 485 SensingMPU();
taknokolat 22:a19be3b505b7 486 wait_ms(30);
taknokolat 22:a19be3b505b7 487 pc.printf("nowAngle_MPU=%f\r\n",nowAngle[YAW]);
taknokolat 22:a19be3b505b7 488 }
taknokolat 22:a19be3b505b7 489 }else{
taknokolat 22:a19be3b505b7 490 while(nowAngle[YAW] > HighTargetYaw || nowAngle[YAW] < LowTargetYaw){
taknokolat 22:a19be3b505b7 491 //MoveCansat('r');
taknokolat 22:a19be3b505b7 492 SensingMPU();
taknokolat 22:a19be3b505b7 493 wait_ms(30);
taknokolat 22:a19be3b505b7 494 pc.printf("nowAngle_MPU=%f\r\n",nowAngle[YAW]);
taknokolat 22:a19be3b505b7 495 }
taknokolat 7:8989a4b84695 496 }
taknokolat 24:0ad1725c7849 497 g_CameraDegCounter = 0;
taknokolat 32:ab030832d180 498
taknokolat 32:ab030832d180 499 NVIC_EnableIRQ(USART6_IRQn);
taknokolat 32:ab030832d180 500 NVIC_EnableIRQ(USART2_IRQn);
taknokolat 7:8989a4b84695 501 return;
taknokolat 7:8989a4b84695 502 }
taknokolat 7:8989a4b84695 503
taknokolat 7:8989a4b84695 504 void FocusAdjust(){
taknokolat 31:210cc32d3175 505 if(FocusFlag == false){
taknokolat 33:5f77118eacd5 506 servoCameraPinto.pulsewidth_us(Focus_NEUTRAL + Pint_speed);
taknokolat 33:5f77118eacd5 507 servoR.pulsewidth_us(Servo_NEUTRAL_R); //servo initialize
taknokolat 33:5f77118eacd5 508 servoL.pulsewidth_us(Servo_NEUTRAL_L);
taknokolat 33:5f77118eacd5 509 wait(Pint_wait);
taknokolat 31:210cc32d3175 510 FocusFlag = !FocusFlag;
taknokolat 31:210cc32d3175 511 }else{
taknokolat 33:5f77118eacd5 512 servoCameraPinto.pulsewidth_us(Focus_NEUTRAL - Pint_speed);
taknokolat 33:5f77118eacd5 513 servoR.pulsewidth_us(Servo_NEUTRAL_R); //servo initialize
taknokolat 33:5f77118eacd5 514 servoL.pulsewidth_us(Servo_NEUTRAL_L);
taknokolat 33:5f77118eacd5 515 wait(Pint_wait);
taknokolat 31:210cc32d3175 516 FocusFlag = !FocusFlag;
taknokolat 31:210cc32d3175 517 }
HARUKIDELTA 17:83db05cab3d1 518 servoCameraPinto.pulsewidth_us(Focus_NEUTRAL);
taknokolat 11:d44d137831b9 519
taknokolat 7:8989a4b84695 520 return;
taknokolat 15:1519fccfcae3 521 }
taknokolat 7:8989a4b84695 522
taknokolat 1:290e621741fd 523 void getSF_Serial_jevois(){
taknokolat 22:a19be3b505b7 524
taknokolat 22:a19be3b505b7 525 //pc.printf("jevois\r\n");
taknokolat 1:290e621741fd 526
taknokolat 1:290e621741fd 527
taknokolat 1:290e621741fd 528 static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
taknokolat 1:290e621741fd 529
taknokolat 1:290e621741fd 530 static int bufcounter=0;
taknokolat 1:290e621741fd 531
taknokolat 1:290e621741fd 532
taknokolat 1:290e621741fd 533
taknokolat 1:290e621741fd 534 if(pc.readable()) { // 受信確認
taknokolat 1:290e621741fd 535
taknokolat 1:290e621741fd 536 SFbuf[bufcounter] = pc.getc(); // 1文字取り出し
taknokolat 1:290e621741fd 537 if(SFbuf[0]!='S'){
taknokolat 1:290e621741fd 538 //pc.printf("x");
taknokolat 1:290e621741fd 539 return;
taknokolat 1:290e621741fd 540 }
taknokolat 1:290e621741fd 541
taknokolat 1:290e621741fd 542
taknokolat 1:290e621741fd 543
taknokolat 22:a19be3b505b7 544 pc.printf("%c",SFbuf[bufcounter]);
taknokolat 1:290e621741fd 545
Skykon 3:c18342e4fddd 546 if(SFbuf[0]=='S' && bufcounter<5)bufcounter++;
taknokolat 1:290e621741fd 547
taknokolat 1:290e621741fd 548 if(bufcounter==5 && SFbuf[4]=='F'){
taknokolat 1:290e621741fd 549
taknokolat 1:290e621741fd 550 g_landingcommand = SFbuf[1];
taknokolat 2:f30666d7838b 551 wait_ms(31);//信号が速すぎることによる割り込み防止
taknokolat 1:290e621741fd 552 //pc.printf("%c",g_landingcommand);
taknokolat 1:290e621741fd 553 //wait_ms(20);
taknokolat 1:290e621741fd 554 //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
taknokolat 1:290e621741fd 555 bufcounter = 0;
taknokolat 1:290e621741fd 556 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 557 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 558 //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
taknokolat 1:290e621741fd 559 }
taknokolat 1:290e621741fd 560
taknokolat 1:290e621741fd 561 else if(bufcounter>=5){
taknokolat 1:290e621741fd 562 //pc.printf("Communication Falsed.\r\n");
taknokolat 1:290e621741fd 563 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 564 bufcounter = 0;
taknokolat 1:290e621741fd 565 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 566 }
taknokolat 1:290e621741fd 567 }
taknokolat 1:290e621741fd 568
taknokolat 1:290e621741fd 569
taknokolat 1:290e621741fd 570 }
taknokolat 1:290e621741fd 571
taknokolat 1:290e621741fd 572
taknokolat 12:083662bca47d 573 void getSF_Serial_pi(){
taknokolat 2:f30666d7838b 574
taknokolat 7:8989a4b84695 575 //NVIC_DisableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 576
taknokolat 1:290e621741fd 577 static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
taknokolat 1:290e621741fd 578
taknokolat 1:290e621741fd 579 static int bufcounter=0;
taknokolat 1:290e621741fd 580
taknokolat 1:290e621741fd 581
taknokolat 1:290e621741fd 582
taknokolat 1:290e621741fd 583 if(pc2.readable()) { // 受信確認
taknokolat 1:290e621741fd 584
taknokolat 1:290e621741fd 585 SFbuf[bufcounter] = pc2.getc(); // 1文字取り出し
taknokolat 1:290e621741fd 586 if(SFbuf[0]!='S'){
taknokolat 1:290e621741fd 587 //pc.printf("x");
taknokolat 1:290e621741fd 588 return;
taknokolat 1:290e621741fd 589 }
taknokolat 1:290e621741fd 590
taknokolat 1:290e621741fd 591
taknokolat 1:290e621741fd 592
taknokolat 30:90252dc48c1a 593 pc.printf("%c",SFbuf[bufcounter]);
taknokolat 1:290e621741fd 594
taknokolat 1:290e621741fd 595 if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++;
taknokolat 1:290e621741fd 596
taknokolat 1:290e621741fd 597 if(bufcounter==5 && SFbuf[4]=='F'){
taknokolat 1:290e621741fd 598
taknokolat 1:290e621741fd 599 g_landingcommand = SFbuf[1];
taknokolat 2:f30666d7838b 600 wait_ms(31);//信号が速すぎることによる割り込み防止
taknokolat 1:290e621741fd 601 //pc.printf("%c",g_landingcommand);
taknokolat 1:290e621741fd 602 //wait_ms(20);
taknokolat 1:290e621741fd 603 //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
taknokolat 1:290e621741fd 604 bufcounter = 0;
taknokolat 1:290e621741fd 605 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 606 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 607 //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
taknokolat 1:290e621741fd 608 }
taknokolat 1:290e621741fd 609
taknokolat 1:290e621741fd 610 else if(bufcounter>=5){
taknokolat 1:290e621741fd 611 //pc.printf("Communication Falsed.\r\n");
taknokolat 1:290e621741fd 612 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 613 bufcounter = 0;
taknokolat 1:290e621741fd 614 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 615 }
taknokolat 1:290e621741fd 616 }
taknokolat 2:f30666d7838b 617
taknokolat 7:8989a4b84695 618 //NVIC_EnableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 619
taknokolat 12:083662bca47d 620 }
taknokolat 1:290e621741fd 621
Skykon 3:c18342e4fddd 622
Skykon 3:c18342e4fddd 623 void setup(){
HARUKIDELTA 23:29b2722bd753 624
HARUKIDELTA 23:29b2722bd753 625 led1 = 1;
HARUKIDELTA 23:29b2722bd753 626 led2 = 1;
HARUKIDELTA 23:29b2722bd753 627 led3 = 1;
HARUKIDELTA 23:29b2722bd753 628 led4 = 1;
HARUKIDELTA 26:ea86e69563b5 629
HARUKIDELTA 26:ea86e69563b5 630 servoR.pulsewidth_us(Servo_NEUTRAL_R); //servo initialize
HARUKIDELTA 26:ea86e69563b5 631 servoL.pulsewidth_us(Servo_NEUTRAL_L);
HARUKIDELTA 26:ea86e69563b5 632 servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
taknokolat 29:5d239812ace6 633 servoCameradeg.pulsewidth_us(Camera_deg_B);
HARUKIDELTA 26:ea86e69563b5 634 servoCameraPinto.pulsewidth_us(focus_NEUTRAL);
Skykon 3:c18342e4fddd 635
taknokolat 13:b088f0db7158 636 SetOptions(&Servo_NEUTRAL_R, &Servo_NEUTRAL_L,
taknokolat 12:083662bca47d 637 &Servo_high_FORWARD_R, &Servo_high_FORWARD_L,
taknokolat 12:083662bca47d 638 &Servo_slow_FORWARD_R,&Servo_slow_FORWARD_L,
taknokolat 12:083662bca47d 639 &Turntable_NEUTRAL,&Matchspeed,&Focus_NEUTRAL,
taknokolat 16:26cee2aaf61d 640 &Camera_deg_A, &Camera_deg_B,
HARUKIDELTA 17:83db05cab3d1 641 &Pint_speed,&Pint_wait,
taknokolat 19:949bab1e9451 642 &Turntable_speed,
taknokolat 24:0ad1725c7849 643 &Time360, &Match_wid,
taknokolat 20:011977a37394 644 &Camera_board_wait
taknokolat 13:b088f0db7158 645 );
taknokolat 12:083662bca47d 646
Skykon 3:c18342e4fddd 647 Init_sensors();
Skykon 3:c18342e4fddd 648 //switch2.rise(ResetTrim);
Skykon 3:c18342e4fddd 649
taknokolat 30:90252dc48c1a 650 //NVIC_SetPriority(USART6_IRQn,0);
taknokolat 7:8989a4b84695 651 //NVIC_SetPriority(EXTI0_IRQn,1);
taknokolat 7:8989a4b84695 652 //NVIC_SetPriority(TIM5_IRQn,2);
taknokolat 7:8989a4b84695 653 //NVIC_SetPriority(EXTI9_5_IRQn,3);
taknokolat 7:8989a4b84695 654 //NVIC_SetPriority(USART2_IRQn,4);
taknokolat 7:8989a4b84695 655
taknokolat 30:90252dc48c1a 656 NVIC_SetPriority(USART6_IRQn,0);//割り込み優先度
taknokolat 7:8989a4b84695 657 NVIC_SetPriority(USART2_IRQn,1);
taknokolat 7:8989a4b84695 658
Skykon 3:c18342e4fddd 659 DisplayClock();
Skykon 3:c18342e4fddd 660 t.start();
Skykon 3:c18342e4fddd 661
Skykon 3:c18342e4fddd 662 pc.printf("MPU calibration start\r\n");
taknokolat 24:0ad1725c7849 663 //pc.printf("HMC calibration start\r\n");
Skykon 3:c18342e4fddd 664
Skykon 3:c18342e4fddd 665 float offsetstart = t.read();
Skykon 3:c18342e4fddd 666 while(t.read() - offsetstart < 26){
Skykon 3:c18342e4fddd 667 SensingMPU();
taknokolat 24:0ad1725c7849 668 //SensingHMC();
Skykon 3:c18342e4fddd 669 for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
taknokolat 24:0ad1725c7849 670 //pc.printf("\t%3.2f\t",nowAngle_HMC);
Skykon 3:c18342e4fddd 671 pc.printf("\r\n");
HARUKIDELTA 23:29b2722bd753 672 led1 = !led1;
HARUKIDELTA 23:29b2722bd753 673 led2 = !led2;
HARUKIDELTA 23:29b2722bd753 674 led3 = !led3;
HARUKIDELTA 23:29b2722bd753 675 led4 = !led4;
Skykon 3:c18342e4fddd 676 }
Skykon 3:c18342e4fddd 677
Skykon 3:c18342e4fddd 678 FirstROLL = nowAngle[ROLL];
Skykon 3:c18342e4fddd 679 FirstPITCH = nowAngle[PITCH];
Skykon 3:c18342e4fddd 680 nowAngle[ROLL] -=FirstROLL;
Skykon 3:c18342e4fddd 681 nowAngle[PITCH] -=FirstPITCH;
taknokolat 6:166746820555 682 FirstYAW = nowAngle[YAW];
taknokolat 6:166746820555 683 nowAngle[YAW] -= FirstYAW;
taknokolat 6:166746820555 684
taknokolat 24:0ad1725c7849 685 //g_FirstYAW_HMC = nowAngle_HMC;
taknokolat 24:0ad1725c7849 686 //nowAngle_HMC -=g_FirstYAW_HMC;
taknokolat 24:0ad1725c7849 687 //if(nowAngle_HMC<0)nowAngle_HMC+=360;
Skykon 3:c18342e4fddd 688
HARUKIDELTA 23:29b2722bd753 689 led1 = 0;
HARUKIDELTA 23:29b2722bd753 690 led2 = 0;
HARUKIDELTA 23:29b2722bd753 691 led3 = 0;
HARUKIDELTA 23:29b2722bd753 692 led4 = 0;
HARUKIDELTA 23:29b2722bd753 693
Skykon 3:c18342e4fddd 694 wait(0.2);
Skykon 3:c18342e4fddd 695
HARUKIDELTA 26:ea86e69563b5 696
HARUKIDELTA 26:ea86e69563b5 697 g_fp = fopen( "/sd/Datalog01.txt" ,"a" );
HARUKIDELTA 26:ea86e69563b5 698 if( g_fp == NULL ) {
HARUKIDELTA 26:ea86e69563b5 699 pc.printf( "ファイルオープンエラー\r\n" );
HARUKIDELTA 26:ea86e69563b5 700 }
HARUKIDELTA 27:f67efcee6509 701 fprintf(g_fp,"\r\n-------------------------\r\n");
HARUKIDELTA 26:ea86e69563b5 702 fprintf(g_fp,"All initialized.\r\n");
HARUKIDELTA 26:ea86e69563b5 703 fclose( g_fp );
HARUKIDELTA 26:ea86e69563b5 704
HARUKIDELTA 26:ea86e69563b5 705
Skykon 3:c18342e4fddd 706 pc.printf("All initialized\r\n");
taknokolat 4:67f705d42f1e 707 setupFlag=true;
taknokolat 1:290e621741fd 708 }
taknokolat 1:290e621741fd 709
Skykon 3:c18342e4fddd 710
Skykon 3:c18342e4fddd 711 void SensingMPU(){
Skykon 3:c18342e4fddd 712 //static int16_t deltaT = 0, t_start = 0;
Skykon 3:c18342e4fddd 713 //t_start = t.read_us();
Skykon 3:c18342e4fddd 714
taknokolat 7:8989a4b84695 715 float rpy[3] = {0};
Skykon 3:c18342e4fddd 716 static uint16_t count_changeRPY = 0;
Skykon 3:c18342e4fddd 717 static bool flg_checkoutlier = false;
taknokolat 30:90252dc48c1a 718 NVIC_DisableIRQ(USART6_IRQn);
Skykon 3:c18342e4fddd 719 NVIC_DisableIRQ(USART2_IRQn);
Skykon 3:c18342e4fddd 720 NVIC_DisableIRQ(TIM5_IRQn);
Skykon 3:c18342e4fddd 721 NVIC_DisableIRQ(EXTI0_IRQn);
Skykon 3:c18342e4fddd 722 NVIC_DisableIRQ(EXTI9_5_IRQn);
Skykon 3:c18342e4fddd 723
Skykon 3:c18342e4fddd 724 mpu6050.getRollPitchYaw_Skipper(rpy);
Skykon 3:c18342e4fddd 725
taknokolat 30:90252dc48c1a 726 NVIC_EnableIRQ(USART6_IRQn);
Skykon 3:c18342e4fddd 727 NVIC_EnableIRQ(USART2_IRQn);
Skykon 3:c18342e4fddd 728 NVIC_EnableIRQ(TIM5_IRQn);
Skykon 3:c18342e4fddd 729 NVIC_EnableIRQ(EXTI0_IRQn);
Skykon 3:c18342e4fddd 730 NVIC_EnableIRQ(EXTI9_5_IRQn);
Skykon 3:c18342e4fddd 731
Skykon 3:c18342e4fddd 732
Skykon 3:c18342e4fddd 733 //外れ値対策
Skykon 3:c18342e4fddd 734 for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI;
Skykon 3:c18342e4fddd 735 rpy[ROLL] -= FirstROLL;
Skykon 3:c18342e4fddd 736 rpy[PITCH] -= FirstPITCH;
taknokolat 4:67f705d42f1e 737 if(!setupFlag){
taknokolat 4:67f705d42f1e 738 rpy[YAW] -= FirstYAW;
taknokolat 4:67f705d42f1e 739 }else{
taknokolat 4:67f705d42f1e 740 if(rpy[YAW] >= FirstYAW){
taknokolat 4:67f705d42f1e 741 rpy[YAW] -= FirstYAW;
taknokolat 4:67f705d42f1e 742 }else{
taknokolat 4:67f705d42f1e 743 rpy[YAW] += 360.0f;
taknokolat 4:67f705d42f1e 744 rpy[YAW] -= FirstYAW;
taknokolat 4:67f705d42f1e 745 }
taknokolat 4:67f705d42f1e 746 }
Skykon 3:c18342e4fddd 747
Skykon 3:c18342e4fddd 748 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 749 if(!flg_checkoutlier || count_changeRPY >= 2){
Skykon 3:c18342e4fddd 750 for(uint8_t i=0; i<3; i++){
taknokolat 34:0c2b67b31b12 751 if(rpy[i] > nowAngle[i]-180 && rpy[i] < nowAngle[i]+180){
taknokolat 34:0c2b67b31b12 752 nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f; //2つの移動平均
taknokolat 34:0c2b67b31b12 753 }else{
taknokolat 34:0c2b67b31b12 754 nowAngle[i] = rpy[i];
taknokolat 34:0c2b67b31b12 755 }
Skykon 3:c18342e4fddd 756 }
Skykon 3:c18342e4fddd 757 count_changeRPY = 0;
Skykon 3:c18342e4fddd 758 }else count_changeRPY++;
Skykon 3:c18342e4fddd 759 flg_checkoutlier = false;
Skykon 3:c18342e4fddd 760
Skykon 3:c18342e4fddd 761 }
Skykon 3:c18342e4fddd 762
Skykon 3:c18342e4fddd 763
Skykon 3:c18342e4fddd 764 void Init_sensors(){
Skykon 3:c18342e4fddd 765 if(mpu6050.setup() == -1){
Skykon 3:c18342e4fddd 766 pc.printf("failed initialize\r\n");
Skykon 3:c18342e4fddd 767 }
Skykon 3:c18342e4fddd 768 }
Skykon 3:c18342e4fddd 769
Skykon 3:c18342e4fddd 770
Skykon 3:c18342e4fddd 771 void DisplayClock(){
Skykon 3:c18342e4fddd 772 pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000);
Skykon 3:c18342e4fddd 773 pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000);
Skykon 3:c18342e4fddd 774 pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000);
Skykon 3:c18342e4fddd 775 pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000);
Skykon 3:c18342e4fddd 776 pc.printf("\r\n");
taknokolat 4:67f705d42f1e 777 }
taknokolat 6:166746820555 778
taknokolat 24:0ad1725c7849 779 /*void SensingHMC(){
taknokolat 6:166746820555 780 //static int16_t deltaT = 0, t_start = 0;
taknokolat 6:166746820555 781 //t_start = t.read_us();
taknokolat 6:166746820555 782
taknokolat 7:8989a4b84695 783 float rpy=0;
taknokolat 6:166746820555 784 static uint16_t count_changeRPY = 0;
taknokolat 6:166746820555 785 static bool flg_checkoutlier = false;
taknokolat 30:90252dc48c1a 786 NVIC_DisableIRQ(USART6_IRQn);
taknokolat 6:166746820555 787 NVIC_DisableIRQ(USART2_IRQn);
taknokolat 19:949bab1e9451 788 //NVIC_DisableIRQ(TIM5_IRQn);
taknokolat 19:949bab1e9451 789 //NVIC_DisableIRQ(EXTI0_IRQn);
taknokolat 19:949bab1e9451 790 //NVIC_DisableIRQ(EXTI9_5_IRQn);
taknokolat 4:67f705d42f1e 791
taknokolat 24:0ad1725c7849 792 rpy= compass.getHeadingXYDeg(Time360,Match_wid);
taknokolat 6:166746820555 793
taknokolat 30:90252dc48c1a 794 NVIC_EnableIRQ(USART6_IRQn);
taknokolat 6:166746820555 795 NVIC_EnableIRQ(USART2_IRQn);
taknokolat 19:949bab1e9451 796 //NVIC_EnableIRQ(TIM5_IRQn);
taknokolat 19:949bab1e9451 797 //NVIC_EnableIRQ(EXTI0_IRQn);
taknokolat 19:949bab1e9451 798 //NVIC_EnableIRQ(EXTI9_5_IRQn);
taknokolat 6:166746820555 799
taknokolat 6:166746820555 800
taknokolat 6:166746820555 801 //外れ値対策
taknokolat 6:166746820555 802 //rpy*= 180.0f/PI;
taknokolat 6:166746820555 803 if(!setupFlag){
taknokolat 6:166746820555 804 rpy -= g_FirstYAW_HMC;
taknokolat 6:166746820555 805 }else{
taknokolat 6:166746820555 806 if(rpy >= g_FirstYAW_HMC){
taknokolat 6:166746820555 807 rpy -= g_FirstYAW_HMC;
taknokolat 6:166746820555 808 }else{
taknokolat 6:166746820555 809 rpy += 360.0f;
taknokolat 6:166746820555 810 rpy -= g_FirstYAW_HMC;
taknokolat 6:166746820555 811 }
taknokolat 6:166746820555 812 }
taknokolat 6:166746820555 813
taknokolat 6:166746820555 814 if(rpy < nowAngle_HMC-10 || rpy > nowAngle_HMC+10) {flg_checkoutlier = true;}
taknokolat 6:166746820555 815 if(!flg_checkoutlier || count_changeRPY >= 2){
taknokolat 6:166746820555 816
taknokolat 6:166746820555 817 nowAngle_HMC = (rpy + nowAngle_HMC)/2.0f; //2つの移動平均
taknokolat 6:166746820555 818
taknokolat 6:166746820555 819 count_changeRPY = 0;
taknokolat 6:166746820555 820 }else count_changeRPY++;
taknokolat 6:166746820555 821 flg_checkoutlier = false;
taknokolat 6:166746820555 822
taknokolat 24:0ad1725c7849 823 }*/
taknokolat 12:083662bca47d 824
taknokolat 12:083662bca47d 825 int SetOptions(int *Servo_NEUTRAL_R, int *Servo_NEUTRAL_L,
taknokolat 12:083662bca47d 826 int *Servo_high_FORWARD_R, int *Servo_high_FORWARD_L,
taknokolat 12:083662bca47d 827 int *Servo_slow_FORWARD_R,int *Servo_slow_FORWARD_L,
taknokolat 12:083662bca47d 828 int *Turntable_NEUTRAL,int *Matchspeed,int *Focus_NEUTRAL,
taknokolat 16:26cee2aaf61d 829 int *Camera_deg_A, int *Camera_deg_B,
HARUKIDELTA 17:83db05cab3d1 830 int *Pint_speed, float *Pint_wait,
taknokolat 19:949bab1e9451 831 int *Turntable_speed,
taknokolat 24:0ad1725c7849 832 int *Time360, int *Match_wid,
taknokolat 20:011977a37394 833 int *Camera_board_wait
taknokolat 12:083662bca47d 834 ){
taknokolat 12:083662bca47d 835
taknokolat 12:083662bca47d 836 pc.printf("SDsetup start.\r\n");
taknokolat 12:083662bca47d 837
taknokolat 12:083662bca47d 838 FILE *fp;
taknokolat 12:083662bca47d 839 char parameter[20]; //文字列渡す用の配列
taknokolat 12:083662bca47d 840 int SDerrorcount = 0; //取得できなかった数を返す
taknokolat 12:083662bca47d 841 const char *paramNames[] = {
taknokolat 12:083662bca47d 842 "SERVO_NEUTRAL_R",
taknokolat 12:083662bca47d 843 "SERVO_NEUTRAL_L",
taknokolat 12:083662bca47d 844 "SERVO_HIGH_FORWARD_R",
taknokolat 12:083662bca47d 845 "SERVO_HIGH_FORWARD_L",
taknokolat 12:083662bca47d 846 "SERVO_SLOW_FORWARD_R",
taknokolat 12:083662bca47d 847 "SERVO_SLOW_FORWARD_L",
taknokolat 12:083662bca47d 848 "TURNTABLE_NEUTRAL",
taknokolat 12:083662bca47d 849 "MATCH_SPEED",
taknokolat 12:083662bca47d 850 "FOCUS_NEUTRAL",
taknokolat 12:083662bca47d 851 "CAMERA_DEG_A",
taknokolat 16:26cee2aaf61d 852 "CAMERA_DEG_B",
taknokolat 16:26cee2aaf61d 853 "PINT_SPEED",
HARUKIDELTA 17:83db05cab3d1 854 "PINT_WAIT",
taknokolat 19:949bab1e9451 855 "TURNTABLE_SPEED",
taknokolat 24:0ad1725c7849 856 "TIME360",
taknokolat 24:0ad1725c7849 857 "MATCH_WID",
taknokolat 20:011977a37394 858 "CAMERA_BOARD_WAIT"
taknokolat 12:083662bca47d 859 };
taknokolat 12:083662bca47d 860
taknokolat 12:083662bca47d 861 fp = fopen("/sd/option.txt","r");
taknokolat 12:083662bca47d 862
taknokolat 12:083662bca47d 863 if(fp != NULL){ //開けたら
taknokolat 12:083662bca47d 864 pc.printf("File was openned.\r\n");
taknokolat 12:083662bca47d 865 if(GetParameter(fp,paramNames[0],parameter)) *Servo_NEUTRAL_R = atof(parameter);
taknokolat 12:083662bca47d 866 else{ *Servo_NEUTRAL_R = servo_NEUTRAL_R;
taknokolat 12:083662bca47d 867 SDerrorcount++;
taknokolat 12:083662bca47d 868 }
taknokolat 12:083662bca47d 869 if(GetParameter(fp,paramNames[1],parameter)) *Servo_NEUTRAL_L = atof(parameter);
taknokolat 12:083662bca47d 870 else{ *Servo_NEUTRAL_L = servo_NEUTRAL_L;
taknokolat 12:083662bca47d 871 SDerrorcount++;
taknokolat 12:083662bca47d 872 }
taknokolat 12:083662bca47d 873 if(GetParameter(fp,paramNames[2],parameter)) *Servo_high_FORWARD_R = atof(parameter);
taknokolat 12:083662bca47d 874 else{ *Servo_high_FORWARD_R = servo_high_FORWARD_R;
taknokolat 12:083662bca47d 875 SDerrorcount++;
taknokolat 12:083662bca47d 876 }
taknokolat 12:083662bca47d 877 if(GetParameter(fp,paramNames[3],parameter)) *Servo_high_FORWARD_L = atof(parameter);
taknokolat 12:083662bca47d 878 else{ *Servo_high_FORWARD_L = servo_high_FORWARD_L;
taknokolat 12:083662bca47d 879 SDerrorcount++;
taknokolat 12:083662bca47d 880 }
taknokolat 12:083662bca47d 881 if(GetParameter(fp,paramNames[4],parameter)) *Servo_slow_FORWARD_R = atof(parameter);
taknokolat 12:083662bca47d 882 else{ *Servo_slow_FORWARD_R = servo_slow_FORWARD_R;
taknokolat 12:083662bca47d 883 SDerrorcount++;
taknokolat 12:083662bca47d 884 }
taknokolat 12:083662bca47d 885 if(GetParameter(fp,paramNames[5],parameter)) *Servo_slow_FORWARD_L = atof(parameter);
taknokolat 12:083662bca47d 886 else{ *Servo_slow_FORWARD_L = servo_slow_FORWARD_L;
taknokolat 12:083662bca47d 887 SDerrorcount++;
taknokolat 12:083662bca47d 888 }
taknokolat 12:083662bca47d 889
taknokolat 12:083662bca47d 890 if(GetParameter(fp,paramNames[6],parameter)) *Turntable_NEUTRAL = atof(parameter);
taknokolat 12:083662bca47d 891 else{ *Turntable_NEUTRAL = turntable_NEUTRAL;
taknokolat 12:083662bca47d 892 SDerrorcount++;
taknokolat 12:083662bca47d 893 }
taknokolat 12:083662bca47d 894 if(GetParameter(fp,paramNames[7],parameter)) *Matchspeed = atof(parameter);
taknokolat 12:083662bca47d 895 else{ *Matchspeed = matchspeed;
taknokolat 12:083662bca47d 896 SDerrorcount++;
taknokolat 12:083662bca47d 897 }
taknokolat 12:083662bca47d 898 if(GetParameter(fp,paramNames[8],parameter)) *Focus_NEUTRAL = atof(parameter);
taknokolat 12:083662bca47d 899 else{ *Focus_NEUTRAL = focus_NEUTRAL;
taknokolat 12:083662bca47d 900 SDerrorcount++;
taknokolat 12:083662bca47d 901 }
taknokolat 19:949bab1e9451 902 if(GetParameter(fp,paramNames[9],parameter)) *Camera_deg_A = atof(parameter);
taknokolat 12:083662bca47d 903 else{ *Camera_deg_A = camera_deg_A;
taknokolat 12:083662bca47d 904 SDerrorcount++;
taknokolat 12:083662bca47d 905 }
taknokolat 19:949bab1e9451 906 if(GetParameter(fp,paramNames[10],parameter)) *Camera_deg_B = atof(parameter);
taknokolat 12:083662bca47d 907 else{ *Camera_deg_B = camera_deg_B;
taknokolat 12:083662bca47d 908 SDerrorcount++;
taknokolat 12:083662bca47d 909 }
taknokolat 19:949bab1e9451 910 if(GetParameter(fp,paramNames[11],parameter)) *Pint_speed = atof(parameter);
taknokolat 16:26cee2aaf61d 911 else{ *Pint_speed = pint_speed;
taknokolat 16:26cee2aaf61d 912 SDerrorcount++;
taknokolat 16:26cee2aaf61d 913 }
taknokolat 20:011977a37394 914 if(GetParameter(fp,paramNames[12],parameter)) *Pint_wait = atof(parameter);
taknokolat 20:011977a37394 915 else{ *Pint_wait = pint_wait;
taknokolat 16:26cee2aaf61d 916 SDerrorcount++;
taknokolat 16:26cee2aaf61d 917 }
taknokolat 19:949bab1e9451 918 if(GetParameter(fp,paramNames[13],parameter)) *Turntable_speed = atof(parameter);
HARUKIDELTA 17:83db05cab3d1 919 else{ *Turntable_speed = turntable_speed;
HARUKIDELTA 17:83db05cab3d1 920 SDerrorcount++;
HARUKIDELTA 17:83db05cab3d1 921 }
taknokolat 24:0ad1725c7849 922 if(GetParameter(fp,paramNames[14],parameter)) *Time360 = atof(parameter);
taknokolat 24:0ad1725c7849 923 else{ *Time360 = time360;
taknokolat 19:949bab1e9451 924 SDerrorcount++;
taknokolat 19:949bab1e9451 925 }
taknokolat 24:0ad1725c7849 926 if(GetParameter(fp,paramNames[15],parameter)) *Match_wid = atof(parameter);
taknokolat 24:0ad1725c7849 927 else{ *Match_wid = match_wid;
taknokolat 19:949bab1e9451 928 SDerrorcount++;
taknokolat 19:949bab1e9451 929 }
taknokolat 20:011977a37394 930 if(GetParameter(fp,paramNames[16],parameter)) *Camera_board_wait = atof(parameter);
taknokolat 20:011977a37394 931 else{ *Camera_board_wait = camera_board_wait;
taknokolat 20:011977a37394 932 SDerrorcount++;
taknokolat 20:011977a37394 933 }
taknokolat 12:083662bca47d 934 fclose(fp);
taknokolat 12:083662bca47d 935
taknokolat 12:083662bca47d 936 }else{ //ファイルがなかったら
taknokolat 12:083662bca47d 937 pc.printf("fp was null.\r\n");
taknokolat 12:083662bca47d 938
taknokolat 12:083662bca47d 939 *Servo_NEUTRAL_R = servo_NEUTRAL_R;
taknokolat 12:083662bca47d 940 *Servo_NEUTRAL_L = servo_NEUTRAL_L;
taknokolat 12:083662bca47d 941 *Servo_high_FORWARD_R = servo_high_FORWARD_R;
taknokolat 12:083662bca47d 942 *Servo_high_FORWARD_L = servo_high_FORWARD_L;
taknokolat 12:083662bca47d 943 *Servo_slow_FORWARD_R = servo_slow_FORWARD_R;
taknokolat 12:083662bca47d 944 *Servo_slow_FORWARD_L = servo_slow_FORWARD_L;
taknokolat 12:083662bca47d 945 *Turntable_NEUTRAL = turntable_NEUTRAL;
taknokolat 12:083662bca47d 946 *Matchspeed = matchspeed;
taknokolat 12:083662bca47d 947 *Focus_NEUTRAL = focus_NEUTRAL;
taknokolat 12:083662bca47d 948 *Camera_deg_A = camera_deg_A;
taknokolat 12:083662bca47d 949 *Camera_deg_B = camera_deg_B;
taknokolat 16:26cee2aaf61d 950 *Pint_speed = pint_speed;
taknokolat 16:26cee2aaf61d 951 *Pint_wait = pint_wait;
HARUKIDELTA 17:83db05cab3d1 952 *Turntable_speed = turntable_speed;
taknokolat 24:0ad1725c7849 953 *Time360 = time360;
taknokolat 24:0ad1725c7849 954 *Match_wid = match_wid;
taknokolat 22:a19be3b505b7 955 *Camera_board_wait = camera_board_wait;
taknokolat 12:083662bca47d 956 SDerrorcount = -1;
taknokolat 12:083662bca47d 957 }
taknokolat 12:083662bca47d 958 pc.printf("SDsetup finished.\r\n");
taknokolat 12:083662bca47d 959 if(SDerrorcount == 0) pc.printf("setting option is success\r\n");
taknokolat 12:083662bca47d 960 else if(SDerrorcount == -1) pc.printf("ERROR 1. cannot open option\r\n");
taknokolat 12:083662bca47d 961 else if(SDerrorcount > 0) pc.printf("ERROR 2. reading parameter is failed[%d]\r\n",SDerrorcount);
taknokolat 12:083662bca47d 962
taknokolat 12:083662bca47d 963 pc.printf("Servo_NEUTRAL_R = %d, Servo_NEUTRAL_L = %d\r\n", *Servo_NEUTRAL_R, *Servo_NEUTRAL_L);
taknokolat 13:b088f0db7158 964 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 965 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 966 pc.printf("Turntable_NEUTRAL = %d, Matchspeed = %d\r\n", *Turntable_NEUTRAL, *Matchspeed);
taknokolat 13:b088f0db7158 967 pc.printf("Focus_NEUTRAL = %d\r\n", *Focus_NEUTRAL);
taknokolat 13:b088f0db7158 968 pc.printf("Camera_deg_A = %d, Camera_deg_B = %d\r\n", *Camera_deg_A, *Camera_deg_B);
taknokolat 16:26cee2aaf61d 969 pc.printf("Pint_speed = %d, Pint_wait = %f\r\n", *Pint_speed, *Pint_wait);
HARUKIDELTA 17:83db05cab3d1 970 pc.printf("Turntable_speed = %d\r\n",*Turntable_speed);
taknokolat 24:0ad1725c7849 971 pc.printf("Time360 = %d, Match_wid = %d\r\n", *Time360, *Match_wid);
taknokolat 20:011977a37394 972 pc.printf("Camera_board_wait = %d\r\n", *Camera_board_wait);
taknokolat 12:083662bca47d 973
taknokolat 12:083662bca47d 974 return SDerrorcount;
taknokolat 12:083662bca47d 975 }
taknokolat 12:083662bca47d 976 int GetParameter(FILE *fp, const char *paramName,char parameter[]){
taknokolat 12:083662bca47d 977 int i=0, j=0;
taknokolat 12:083662bca47d 978 int strmax = 200;
taknokolat 12:083662bca47d 979 char str[strmax];
taknokolat 12:083662bca47d 980
taknokolat 12:083662bca47d 981 rewind(fp); //ファイル位置を先頭に
taknokolat 12:083662bca47d 982 while(1){
taknokolat 12:083662bca47d 983 if (fgets(str, strmax, fp) == NULL) {
taknokolat 12:083662bca47d 984 return 0;
taknokolat 12:083662bca47d 985 }
taknokolat 12:083662bca47d 986 if (!strncmp(str, paramName, strlen(paramName))) {
taknokolat 12:083662bca47d 987 while (str[i++] != '=') {}
taknokolat 12:083662bca47d 988 while (str[i] != '\n') {
taknokolat 12:083662bca47d 989 parameter[j++] = str[i++];
taknokolat 12:083662bca47d 990 }
taknokolat 12:083662bca47d 991 parameter[j] = '\0';
taknokolat 12:083662bca47d 992 return 1;
taknokolat 12:083662bca47d 993 }
taknokolat 12:083662bca47d 994 }
taknokolat 12:083662bca47d 995 }
taknokolat 12:083662bca47d 996
taknokolat 12:083662bca47d 997
taknokolat 6:166746820555 998
taknokolat 4:67f705d42f1e 999 void DebugPrint(){
taknokolat 4:67f705d42f1e 1000 //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); //skipper地磁気センサ_デバック用
taknokolat 7:8989a4b84695 1001 //pc.printf("%3.2f\t",nowAngle[2]);
taknokolat 7:8989a4b84695 1002 //pc.printf("%3.2f\t",nowAngle_HMC); //HMC
taknokolat 7:8989a4b84695 1003 //pc.printf("\r\n");
taknokolat 6:166746820555 1004 }