航空研究会 / Mbed 2 deprecated sahaquiel_1

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@41:d142afd3c2f7, 2019-03-07 (annotated)

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