航空研究会 / Mbed 2 deprecated sahaquiel_brushless_tset

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed MPU6050_2 HMC5883L_4 SDFileSystem3

main.cpp@38:7c323abc62fb, 2019-03-04 (annotated)

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