imano yuta / Mbed 2 deprecated sahaquiel_raspi_uart_test

skipper_raspi_uart_test

Dependencies:   mbed MPU6050_2 HMC5883L_4 SDFileSystem3

main.cpp@24:0ad1725c7849, 2019-02-18 (annotated)

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