imano yuta / Mbed 2 deprecated sahaquiel_raspi_uart_test

skipper_raspi_uart_test

Dependencies:   mbed MPU6050_2 HMC5883L_4 SDFileSystem3

main.cpp@14:90e9b44eb819, 2019-02-12 (annotated)

Committer:
taknokolat
Date:
Tue Feb 12 13:31:33 2019 +0000
Revision:
14:90e9b44eb819
Parent:
13:b088f0db7158
Child:
15:1519fccfcae3
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
taknokolat 11:d44d137831b9 11 #define servo_NEUTRAL_R 1614
taknokolat 11:d44d137831b9 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_low_FORWARD_R 1560 //RasPi速さ調節用
taknokolat 12:083662bca47d 16 #define servo_high_FORWARD_R 1860
taknokolat 12:083662bca47d 17 #define servo_low_FORWARD_L 1560
taknokolat 12:083662bca47d 18 #define servo_high_FORWARD_L 1860
taknokolat 12:083662bca47d 19 #define servo_low_back_R 1360
taknokolat 12:083662bca47d 20 #define servo_high_back_R 1160
taknokolat 12:083662bca47d 21 #define servo_low_back_L 1360
taknokolat 12:083662bca47d 22 #define servo_high_back_L 1160
taknokolat 12:083662bca47d 23 #define turntable_NEUTRAL 1180 //カメラ台座のサーボ
taknokolat 12:083662bca47d 24 #define matchspeed 1500 + 100 //カメラと方向を合わせるときの車輪の速度
taknokolat 12:083662bca47d 25 #define focus_NEUTRAL 1455 //焦点合わせ用サーボ
taknokolat 12:083662bca47d 26 #define camera_deg_A 1400 //カメラ角度調整
taknokolat 12:083662bca47d 27 #define camera_deg_B 1800
HARUKIDELTA 0:84ddd6d354e1 28
HARUKIDELTA 0:84ddd6d354e1 29 #define MOVE_NEUTRAL 0
HARUKIDELTA 0:84ddd6d354e1 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 12:083662bca47d 63 int *Camera_deg_A, int *Camera_deg_B
taknokolat 12:083662bca47d 64 );
taknokolat 12:083662bca47d 65
taknokolat 6:166746820555 66 static float nowAngle[3] = {0,0,0},nowAngle_HMC=0;
taknokolat 6:166746820555 67 float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0,g_FirstYAW_HMC;
Skykon 3:c18342e4fddd 68
taknokolat 7:8989a4b84695 69 bool setupFlag = false;
taknokolat 7:8989a4b84695 70 bool CameraDegFlag = false;
taknokolat 7:8989a4b84695 71 bool jevoisFlag = true;
taknokolat 4:67f705d42f1e 72
Skykon 3:c18342e4fddd 73 enum Angle{ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度
Skykon 3:c18342e4fddd 74
Skykon 3:c18342e4fddd 75 Timer t;
Skykon 3:c18342e4fddd 76
taknokolat 14:90e9b44eb819 77 SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd");
taknokolat 14:90e9b44eb819 78
taknokolat 11:d44d137831b9 79 RawSerial pc(PA_2,PA_3,115200); //uart2
taknokolat 11:d44d137831b9 80 //pa2:UART2_TX,pa3:UART2_RX
taknokolat 12:083662bca47d 81 RawSerial pc2(PA_11,PA_12,115200); //uart1
taknokolat 11:d44d137831b9 82 //pb6:UART1_TX,pb7:UART1_RX
taknokolat 11:d44d137831b9 83
HARUKIDELTA 5:8bfe95431ec0 84 //PWM pin宣言
taknokolat 6:166746820555 85 PwmOut servoR(PC_6); //TIM3_CH1 車輪右
taknokolat 6:166746820555 86 PwmOut servoL(PC_7); //TIM3_CH2 車輪左
HARUKIDELTA 5:8bfe95431ec0 87 PwmOut servoTurnTable(PB_0); //TIM3_CH3 カメラ台回転Servo
HARUKIDELTA 5:8bfe95431ec0 88 PwmOut servoCameradeg(PB_1); //TIM3_CH4 カメラ角度調節Servo
HARUKIDELTA 5:8bfe95431ec0 89 PwmOut servoCameraPinto(PB_6); //TIM4_CH1 カメラピント合わせ
HARUKIDELTA 5:8bfe95431ec0 90 PwmOut servoCameramount(PA_6); //skipperウラ カメラマウント起動
HARUKIDELTA 5:8bfe95431ec0 91 PwmOut servoGetUP(PC_8); //skipperウラ 起き上がり動作
Skykon 3:c18342e4fddd 92
HARUKIDELTA 5:8bfe95431ec0 93 /*通信用のpinは
HARUKIDELTA 5:8bfe95431ec0 94 PA_3(UART2_Rx) skipperウラ
HARUKIDELTA 5:8bfe95431ec0 95 PA_12(UART6_Rx) skipperオモテ USB端子より
HARUKIDELTA 5:8bfe95431ec0 96 PB_7 (UART1_Rx) skipperオモテ 6番目 TIM4_CH2
HARUKIDELTA 5:8bfe95431ec0 97 */
HARUKIDELTA 5:8bfe95431ec0 98
HARUKIDELTA 5:8bfe95431ec0 99 /*超音波はRaspberryPiに積む*/
HARUKIDELTA 5:8bfe95431ec0 100
HARUKIDELTA 5:8bfe95431ec0 101 //外付けコンパス
taknokolat 6:166746820555 102 HMC5883L compass(PB_9, PB_8); //コンパスセンサー TIM4_CH3とCH4
HARUKIDELTA 0:84ddd6d354e1 103
taknokolat 2:f30666d7838b 104 char g_landingcommand='N';
taknokolat 1:290e621741fd 105
Skykon 3:c18342e4fddd 106 //MPU_check用
Skykon 3:c18342e4fddd 107 MPU6050DMP6 mpu6050(PC_0,&pc);
Skykon 3:c18342e4fddd 108
Skykon 3:c18342e4fddd 109
HARUKIDELTA 0:84ddd6d354e1 110 int main() {
taknokolat 1:290e621741fd 111
Skykon 3:c18342e4fddd 112 //MPU_check
Skykon 3:c18342e4fddd 113 setup();
Skykon 3:c18342e4fddd 114
taknokolat 11:d44d137831b9 115 servoCameradeg.pulsewidth_us(1400);
taknokolat 11:d44d137831b9 116
taknokolat 1:290e621741fd 117 // シリアル通信受信の割り込みイベント登録
taknokolat 1:290e621741fd 118 pc.attach(getSF_Serial_jevois, Serial::RxIrq);
taknokolat 12:083662bca47d 119 pc2.attach(getSF_Serial_pi, Serial::RxIrq);
taknokolat 1:290e621741fd 120
HARUKIDELTA 0:84ddd6d354e1 121 while(1) {
taknokolat 7:8989a4b84695 122 if(jevoisFlag==true) NVIC_DisableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 123 else NVIC_DisableIRQ(USART2_IRQn);
taknokolat 8:d11a59d2a2f1 124
taknokolat 10:63fe920595a7 125 MoveCameraBoard();
taknokolat 10:63fe920595a7 126
taknokolat 7:8989a4b84695 127 if(g_landingcommand!='N') MatchPosition();
taknokolat 8:d11a59d2a2f1 128
taknokolat 8:d11a59d2a2f1 129 if(jevoisFlag==true) NVIC_DisableIRQ(USART1_IRQn);
taknokolat 8:d11a59d2a2f1 130 else NVIC_DisableIRQ(USART2_IRQn);
taknokolat 8:d11a59d2a2f1 131
taknokolat 7:8989a4b84695 132 MoveCansat(g_landingcommand);
Skykon 3:c18342e4fddd 133 wait_ms(23);
taknokolat 8:d11a59d2a2f1 134
taknokolat 7:8989a4b84695 135 if(jevoisFlag==true) NVIC_EnableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 136 else NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 137 }
HARUKIDELTA 0:84ddd6d354e1 138 }
HARUKIDELTA 0:84ddd6d354e1 139
Skykon 3:c18342e4fddd 140
taknokolat 1:290e621741fd 141 void MoveCansat(char g_landingcommand)
HARUKIDELTA 0:84ddd6d354e1 142 {
taknokolat 2:f30666d7838b 143 //NVIC_DisableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 144 //NVIC_DisableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 145 switch(g_landingcommand){
taknokolat 2:f30666d7838b 146 case 'N': //MOVE_NEUTRAL
HARUKIDELTA 0:84ddd6d354e1 147 servoR.pulsewidth_us(servo_NEUTRAL_R);
HARUKIDELTA 0:84ddd6d354e1 148 servoL.pulsewidth_us(servo_NEUTRAL_L);
taknokolat 2:f30666d7838b 149 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 150 //NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 151 break;
taknokolat 1:290e621741fd 152
taknokolat 2:f30666d7838b 153 case 'Y': //MOVE_FORWARD
taknokolat 12:083662bca47d 154 servoR.pulsewidth_us(servo_high_FORWARD_R);
taknokolat 12:083662bca47d 155 servoL.pulsewidth_us(servo_high_FORWARD_L);
taknokolat 2:f30666d7838b 156 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 157 //NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 158 break;
taknokolat 1:290e621741fd 159
taknokolat 12:083662bca47d 160 case 'l': //MOVE_LEFT Low Speed
taknokolat 12:083662bca47d 161 servoR.pulsewidth_us(servo_low_FORWARD_R);
taknokolat 12:083662bca47d 162 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 12:083662bca47d 163 //NVIC_EnableIRQ(USART2_IRQn);
taknokolat 12:083662bca47d 164
taknokolat 12:083662bca47d 165 case 'L': //MOVE_LEFT High Speed
taknokolat 12:083662bca47d 166 servoR.pulsewidth_us(servo_high_FORWARD_R);
taknokolat 2:f30666d7838b 167 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 168 //NVIC_EnableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 169
taknokolat 12:083662bca47d 170 case 'r': //MOVE_RIGHT Low Speed
taknokolat 12:083662bca47d 171 servoL.pulsewidth_us(servo_low_FORWARD_L);
taknokolat 2:f30666d7838b 172 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 173 //NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 174 break;
taknokolat 12:083662bca47d 175
taknokolat 12:083662bca47d 176 case 'R': //MOVE_RIGHT High Speed
taknokolat 12:083662bca47d 177 servoL.pulsewidth_us(servo_high_FORWARD_L);
taknokolat 2:f30666d7838b 178 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 179 //NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 180 break;
taknokolat 1:290e621741fd 181
taknokolat 1:290e621741fd 182 case 'G': //GOAL_FORWARD
HARUKIDELTA 0:84ddd6d354e1 183 servoR.pulsewidth_us(servo_slow_FORWARD_R);
HARUKIDELTA 0:84ddd6d354e1 184 servoL.pulsewidth_us(servo_slow_FORWARD_L);
taknokolat 2:f30666d7838b 185 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 186 //NVIC_EnableIRQ(USART2_IRQn);
taknokolat 7:8989a4b84695 187 break;
taknokolat 12:083662bca47d 188
taknokolat 12:083662bca47d 189 case '1': //MOVE_FORWARD Speed Level 1
taknokolat 12:083662bca47d 190 servoR.pulsewidth_us(servo_high_FORWARD_R);
taknokolat 12:083662bca47d 191 servoL.pulsewidth_us(servo_high_FORWARD_L);
taknokolat 12:083662bca47d 192 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 12:083662bca47d 193 wait(1);
taknokolat 12:083662bca47d 194 do{
taknokolat 12:083662bca47d 195 }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
taknokolat 12:083662bca47d 196 //NVIC_EnableIRQ(USART2_IRQn);
taknokolat 12:083662bca47d 197 break;
taknokolat 12:083662bca47d 198
taknokolat 12:083662bca47d 199 case '2': //MOVE_FORWARD Speed Level 2
taknokolat 12:083662bca47d 200 servoR.pulsewidth_us(servo_high_FORWARD_R);
taknokolat 12:083662bca47d 201 servoL.pulsewidth_us(servo_high_FORWARD_L);
taknokolat 12:083662bca47d 202 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 12:083662bca47d 203 wait(2);
taknokolat 12:083662bca47d 204 do{
taknokolat 12:083662bca47d 205 }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
taknokolat 12:083662bca47d 206 //NVIC_EnableIRQ(USART2_IRQn);
taknokolat 12:083662bca47d 207 break;
taknokolat 12:083662bca47d 208
taknokolat 12:083662bca47d 209 case '3': //MOVE_FORWARD Speed Level 3
taknokolat 12:083662bca47d 210 servoR.pulsewidth_us(servo_high_FORWARD_R);
taknokolat 12:083662bca47d 211 servoL.pulsewidth_us(servo_high_FORWARD_L);
taknokolat 12:083662bca47d 212 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 12:083662bca47d 213 wait(3);
taknokolat 12:083662bca47d 214 do{
taknokolat 12:083662bca47d 215 }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
taknokolat 12:083662bca47d 216 //NVIC_EnableIRQ(USART2_IRQn);
taknokolat 12:083662bca47d 217 break;
taknokolat 12:083662bca47d 218
taknokolat 12:083662bca47d 219 case '4': //MOVE_FORWARD Speed Level 4
taknokolat 12:083662bca47d 220 servoR.pulsewidth_us(servo_high_FORWARD_R);
taknokolat 12:083662bca47d 221 servoL.pulsewidth_us(servo_high_FORWARD_L);
taknokolat 12:083662bca47d 222 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 12:083662bca47d 223 wait(4);
taknokolat 12:083662bca47d 224 do{
taknokolat 12:083662bca47d 225 }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
taknokolat 12:083662bca47d 226 //NVIC_EnableIRQ(USART2_IRQn);
taknokolat 12:083662bca47d 227 break;
taknokolat 7:8989a4b84695 228
taknokolat 12:083662bca47d 229 case '5': //MOVE_FORWARD Speed Level 5
taknokolat 12:083662bca47d 230 servoR.pulsewidth_us(servo_high_FORWARD_R);
taknokolat 12:083662bca47d 231 servoL.pulsewidth_us(servo_high_FORWARD_L);
taknokolat 7:8989a4b84695 232 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 233 wait(5);
taknokolat 10:63fe920595a7 234 do{
taknokolat 10:63fe920595a7 235 }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
taknokolat 7:8989a4b84695 236 //NVIC_EnableIRQ(USART2_IRQn);
taknokolat 7:8989a4b84695 237 break;
taknokolat 7:8989a4b84695 238
taknokolat 7:8989a4b84695 239 case 'M': //MatchPosition
taknokolat 12:083662bca47d 240 servoR.pulsewidth_us(matchspeed);
taknokolat 7:8989a4b84695 241 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 242 //NVIC_EnableIRQ(USART2_IRQn);
taknokolat 7:8989a4b84695 243 break;
taknokolat 7:8989a4b84695 244
taknokolat 12:083662bca47d 245 case 'P': //jevoisからraspberry piへの切り替え
taknokolat 7:8989a4b84695 246 jevoisFlag = false;
taknokolat 7:8989a4b84695 247 //NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 248 break;
taknokolat 12:083662bca47d 249
taknokolat 12:083662bca47d 250 case 'S':
taknokolat 12:083662bca47d 251 /*RasPiからの超音波判定(プログラムスタート部)*/
taknokolat 12:083662bca47d 252 break;
taknokolat 1:290e621741fd 253
taknokolat 1:290e621741fd 254 default :
taknokolat 2:f30666d7838b 255 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 256 //NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 257 break;
taknokolat 1:290e621741fd 258
HARUKIDELTA 0:84ddd6d354e1 259 }
taknokolat 7:8989a4b84695 260
HARUKIDELTA 0:84ddd6d354e1 261 return;
taknokolat 1:290e621741fd 262 }
taknokolat 1:290e621741fd 263
taknokolat 7:8989a4b84695 264 void MoveCameraBoard(){
taknokolat 7:8989a4b84695 265 MoveCansat('N');
taknokolat 10:63fe920595a7 266 g_landingcommand='N';
taknokolat 11:d44d137831b9 267 servoTurnTable.pulsewidth_us(1800);
taknokolat 11:d44d137831b9 268 wait_ms(600);
taknokolat 12:083662bca47d 269 servoTurnTable.pulsewidth_us(turntable_NEUTRAL);
taknokolat 7:8989a4b84695 270 if(jevoisFlag == true) FocusAdjust();
taknokolat 7:8989a4b84695 271 else wait(1);
taknokolat 7:8989a4b84695 272
taknokolat 7:8989a4b84695 273 if(g_landingcommand!='N') return;
taknokolat 7:8989a4b84695 274 if(!CameraDegFlag){
taknokolat 7:8989a4b84695 275 servoCameradeg.pulsewidth_us(1800);
taknokolat 7:8989a4b84695 276 CameraDegFlag=!CameraDegFlag;
taknokolat 7:8989a4b84695 277 }else{
taknokolat 11:d44d137831b9 278 servoCameradeg.pulsewidth_us(1400);
taknokolat 7:8989a4b84695 279 CameraDegFlag = !CameraDegFlag;
taknokolat 7:8989a4b84695 280 }
taknokolat 7:8989a4b84695 281
taknokolat 7:8989a4b84695 282 if(jevoisFlag == true) FocusAdjust();
taknokolat 7:8989a4b84695 283 else wait(1);
taknokolat 7:8989a4b84695 284
taknokolat 7:8989a4b84695 285 return;
taknokolat 7:8989a4b84695 286 }
taknokolat 7:8989a4b84695 287
taknokolat 7:8989a4b84695 288 void MatchPosition(){
taknokolat 7:8989a4b84695 289 SensingMPU();
taknokolat 7:8989a4b84695 290 SensingHMC();
taknokolat 7:8989a4b84695 291 DebugPrint();
taknokolat 7:8989a4b84695 292
taknokolat 9:21cd5d18ad9e 293 while(nowAngle[YAW] <= nowAngle_HMC+5 && nowAngle[YAW] >= nowAngle_HMC-5){
taknokolat 7:8989a4b84695 294 MoveCansat('M');
taknokolat 9:21cd5d18ad9e 295 SensingMPU();
taknokolat 7:8989a4b84695 296 }
taknokolat 7:8989a4b84695 297 return;
taknokolat 7:8989a4b84695 298 }
taknokolat 7:8989a4b84695 299
taknokolat 7:8989a4b84695 300 void FocusAdjust(){
taknokolat 11:d44d137831b9 301 servoCameraPinto.pulsewidth_us(Focus_NEUTRAL-200);
taknokolat 11:d44d137831b9 302 pc.printf("set\r\n");
taknokolat 11:d44d137831b9 303 wait(1);
taknokolat 7:8989a4b84695 304
taknokolat 11:d44d137831b9 305 servoCameraPinto.pulsewidth_us(Focus_NEUTRAL+30);
taknokolat 11:d44d137831b9 306 pc.printf("check\r\n");
taknokolat 11:d44d137831b9 307 wait(3);
taknokolat 11:d44d137831b9 308 servoCameraPinto.pulsewidth_us(Focus_NEUTRAL);
taknokolat 11:d44d137831b9 309
taknokolat 7:8989a4b84695 310 return;
taknokolat 7:8989a4b84695 311 }
taknokolat 7:8989a4b84695 312
taknokolat 1:290e621741fd 313 void getSF_Serial_jevois(){
taknokolat 1:290e621741fd 314
taknokolat 1:290e621741fd 315
taknokolat 1:290e621741fd 316 static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
taknokolat 1:290e621741fd 317
taknokolat 1:290e621741fd 318 static int bufcounter=0;
taknokolat 1:290e621741fd 319
taknokolat 1:290e621741fd 320
taknokolat 1:290e621741fd 321
taknokolat 1:290e621741fd 322 if(pc.readable()) { // 受信確認
taknokolat 1:290e621741fd 323
taknokolat 1:290e621741fd 324 SFbuf[bufcounter] = pc.getc(); // 1文字取り出し
taknokolat 1:290e621741fd 325 if(SFbuf[0]!='S'){
taknokolat 1:290e621741fd 326 //pc.printf("x");
taknokolat 1:290e621741fd 327 return;
taknokolat 1:290e621741fd 328 }
taknokolat 1:290e621741fd 329
taknokolat 1:290e621741fd 330
taknokolat 1:290e621741fd 331
taknokolat 1:290e621741fd 332 //pc.printf("%c",SFbuf[bufcounter]);
taknokolat 1:290e621741fd 333
Skykon 3:c18342e4fddd 334 if(SFbuf[0]=='S' && bufcounter<5)bufcounter++;
taknokolat 1:290e621741fd 335
taknokolat 1:290e621741fd 336 if(bufcounter==5 && SFbuf[4]=='F'){
taknokolat 1:290e621741fd 337
taknokolat 1:290e621741fd 338 g_landingcommand = SFbuf[1];
taknokolat 2:f30666d7838b 339 wait_ms(31);//信号が速すぎることによる割り込み防止
taknokolat 1:290e621741fd 340 //pc.printf("%c",g_landingcommand);
taknokolat 1:290e621741fd 341 //wait_ms(20);
taknokolat 1:290e621741fd 342 //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
taknokolat 1:290e621741fd 343 bufcounter = 0;
taknokolat 1:290e621741fd 344 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 345 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 346 //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
taknokolat 1:290e621741fd 347 }
taknokolat 1:290e621741fd 348
taknokolat 1:290e621741fd 349 else if(bufcounter>=5){
taknokolat 1:290e621741fd 350 //pc.printf("Communication Falsed.\r\n");
taknokolat 1:290e621741fd 351 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 352 bufcounter = 0;
taknokolat 1:290e621741fd 353 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 354 }
taknokolat 1:290e621741fd 355 }
taknokolat 1:290e621741fd 356
taknokolat 1:290e621741fd 357
taknokolat 1:290e621741fd 358 }
taknokolat 1:290e621741fd 359
taknokolat 1:290e621741fd 360
taknokolat 12:083662bca47d 361 void getSF_Serial_pi(){
taknokolat 2:f30666d7838b 362
taknokolat 7:8989a4b84695 363 //NVIC_DisableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 364
taknokolat 1:290e621741fd 365 static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
taknokolat 1:290e621741fd 366
taknokolat 1:290e621741fd 367 static int bufcounter=0;
taknokolat 1:290e621741fd 368
taknokolat 1:290e621741fd 369
taknokolat 1:290e621741fd 370
taknokolat 1:290e621741fd 371 if(pc2.readable()) { // 受信確認
taknokolat 1:290e621741fd 372
taknokolat 1:290e621741fd 373 SFbuf[bufcounter] = pc2.getc(); // 1文字取り出し
taknokolat 1:290e621741fd 374 if(SFbuf[0]!='S'){
taknokolat 1:290e621741fd 375 //pc.printf("x");
taknokolat 1:290e621741fd 376 return;
taknokolat 1:290e621741fd 377 }
taknokolat 1:290e621741fd 378
taknokolat 1:290e621741fd 379
taknokolat 1:290e621741fd 380
taknokolat 1:290e621741fd 381 //pc.printf("%c",SFbuf[bufcounter]);
taknokolat 1:290e621741fd 382
taknokolat 1:290e621741fd 383 if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++;
taknokolat 1:290e621741fd 384
taknokolat 1:290e621741fd 385 if(bufcounter==5 && SFbuf[4]=='F'){
taknokolat 1:290e621741fd 386
taknokolat 1:290e621741fd 387 g_landingcommand = SFbuf[1];
taknokolat 2:f30666d7838b 388 wait_ms(31);//信号が速すぎることによる割り込み防止
taknokolat 1:290e621741fd 389 //pc.printf("%c",g_landingcommand);
taknokolat 1:290e621741fd 390 //wait_ms(20);
taknokolat 1:290e621741fd 391 //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
taknokolat 1:290e621741fd 392 bufcounter = 0;
taknokolat 1:290e621741fd 393 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 394 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 395 //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
taknokolat 1:290e621741fd 396 }
taknokolat 1:290e621741fd 397
taknokolat 1:290e621741fd 398 else if(bufcounter>=5){
taknokolat 1:290e621741fd 399 //pc.printf("Communication Falsed.\r\n");
taknokolat 1:290e621741fd 400 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 401 bufcounter = 0;
taknokolat 1:290e621741fd 402 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 403 }
taknokolat 1:290e621741fd 404 }
taknokolat 2:f30666d7838b 405
taknokolat 7:8989a4b84695 406 //NVIC_EnableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 407
taknokolat 12:083662bca47d 408 }
taknokolat 1:290e621741fd 409
Skykon 3:c18342e4fddd 410
Skykon 3:c18342e4fddd 411 void setup(){
Skykon 3:c18342e4fddd 412
taknokolat 13:b088f0db7158 413 SetOptions(&Servo_NEUTRAL_R, &Servo_NEUTRAL_L,
taknokolat 12:083662bca47d 414 &Servo_high_FORWARD_R, &Servo_high_FORWARD_L,
taknokolat 12:083662bca47d 415 &Servo_slow_FORWARD_R,&Servo_slow_FORWARD_L,
taknokolat 12:083662bca47d 416 &Turntable_NEUTRAL,&Matchspeed,&Focus_NEUTRAL,
taknokolat 12:083662bca47d 417 &Camera_deg_A, &Camera_deg_B
taknokolat 13:b088f0db7158 418 );
taknokolat 12:083662bca47d 419
Skykon 3:c18342e4fddd 420 Init_sensors();
Skykon 3:c18342e4fddd 421 //switch2.rise(ResetTrim);
Skykon 3:c18342e4fddd 422
taknokolat 7:8989a4b84695 423 //NVIC_SetPriority(USART1_IRQn,0);
taknokolat 7:8989a4b84695 424 //NVIC_SetPriority(EXTI0_IRQn,1);
taknokolat 7:8989a4b84695 425 //NVIC_SetPriority(TIM5_IRQn,2);
taknokolat 7:8989a4b84695 426 //NVIC_SetPriority(EXTI9_5_IRQn,3);
taknokolat 7:8989a4b84695 427 //NVIC_SetPriority(USART2_IRQn,4);
taknokolat 7:8989a4b84695 428
taknokolat 7:8989a4b84695 429 NVIC_SetPriority(USART1_IRQn,0);//割り込み優先度
taknokolat 7:8989a4b84695 430 NVIC_SetPriority(USART2_IRQn,1);
taknokolat 7:8989a4b84695 431
Skykon 3:c18342e4fddd 432 DisplayClock();
Skykon 3:c18342e4fddd 433 t.start();
Skykon 3:c18342e4fddd 434
Skykon 3:c18342e4fddd 435 pc.printf("MPU calibration start\r\n");
taknokolat 6:166746820555 436 pc.printf("HMC calibration start\r\n");
Skykon 3:c18342e4fddd 437
Skykon 3:c18342e4fddd 438 float offsetstart = t.read();
Skykon 3:c18342e4fddd 439 while(t.read() - offsetstart < 26){
Skykon 3:c18342e4fddd 440 SensingMPU();
taknokolat 6:166746820555 441 SensingHMC();
Skykon 3:c18342e4fddd 442 for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
taknokolat 6:166746820555 443 pc.printf("%3.2f\t",nowAngle_HMC);
Skykon 3:c18342e4fddd 444 pc.printf("\r\n");
Skykon 3:c18342e4fddd 445 }
Skykon 3:c18342e4fddd 446
Skykon 3:c18342e4fddd 447 FirstROLL = nowAngle[ROLL];
Skykon 3:c18342e4fddd 448 FirstPITCH = nowAngle[PITCH];
Skykon 3:c18342e4fddd 449 nowAngle[ROLL] -=FirstROLL;
Skykon 3:c18342e4fddd 450 nowAngle[PITCH] -=FirstPITCH;
taknokolat 6:166746820555 451 FirstYAW = nowAngle[YAW];
taknokolat 6:166746820555 452 nowAngle[YAW] -= FirstYAW;
taknokolat 6:166746820555 453
taknokolat 6:166746820555 454 g_FirstYAW_HMC = nowAngle_HMC;
taknokolat 6:166746820555 455 nowAngle_HMC -=g_FirstYAW_HMC;
taknokolat 6:166746820555 456 if(nowAngle_HMC<0)nowAngle_HMC+=360;
Skykon 3:c18342e4fddd 457
Skykon 3:c18342e4fddd 458 wait(0.2);
Skykon 3:c18342e4fddd 459
Skykon 3:c18342e4fddd 460 pc.printf("All initialized\r\n");
taknokolat 4:67f705d42f1e 461 setupFlag=true;
taknokolat 1:290e621741fd 462 }
taknokolat 1:290e621741fd 463
Skykon 3:c18342e4fddd 464
Skykon 3:c18342e4fddd 465 void SensingMPU(){
Skykon 3:c18342e4fddd 466 //static int16_t deltaT = 0, t_start = 0;
Skykon 3:c18342e4fddd 467 //t_start = t.read_us();
Skykon 3:c18342e4fddd 468
taknokolat 7:8989a4b84695 469 float rpy[3] = {0};
Skykon 3:c18342e4fddd 470 static uint16_t count_changeRPY = 0;
Skykon 3:c18342e4fddd 471 static bool flg_checkoutlier = false;
Skykon 3:c18342e4fddd 472 NVIC_DisableIRQ(USART1_IRQn);
Skykon 3:c18342e4fddd 473 NVIC_DisableIRQ(USART2_IRQn);
Skykon 3:c18342e4fddd 474 NVIC_DisableIRQ(TIM5_IRQn);
Skykon 3:c18342e4fddd 475 NVIC_DisableIRQ(EXTI0_IRQn);
Skykon 3:c18342e4fddd 476 NVIC_DisableIRQ(EXTI9_5_IRQn);
Skykon 3:c18342e4fddd 477
Skykon 3:c18342e4fddd 478 mpu6050.getRollPitchYaw_Skipper(rpy);
Skykon 3:c18342e4fddd 479
Skykon 3:c18342e4fddd 480 NVIC_EnableIRQ(USART1_IRQn);
Skykon 3:c18342e4fddd 481 NVIC_EnableIRQ(USART2_IRQn);
Skykon 3:c18342e4fddd 482 NVIC_EnableIRQ(TIM5_IRQn);
Skykon 3:c18342e4fddd 483 NVIC_EnableIRQ(EXTI0_IRQn);
Skykon 3:c18342e4fddd 484 NVIC_EnableIRQ(EXTI9_5_IRQn);
Skykon 3:c18342e4fddd 485
Skykon 3:c18342e4fddd 486
Skykon 3:c18342e4fddd 487 //外れ値対策
Skykon 3:c18342e4fddd 488 for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI;
Skykon 3:c18342e4fddd 489 rpy[ROLL] -= FirstROLL;
Skykon 3:c18342e4fddd 490 rpy[PITCH] -= FirstPITCH;
taknokolat 4:67f705d42f1e 491 if(!setupFlag){
taknokolat 4:67f705d42f1e 492 rpy[YAW] -= FirstYAW;
taknokolat 4:67f705d42f1e 493 }else{
taknokolat 4:67f705d42f1e 494 if(rpy[YAW] >= FirstYAW){
taknokolat 4:67f705d42f1e 495 rpy[YAW] -= FirstYAW;
taknokolat 4:67f705d42f1e 496 }else{
taknokolat 4:67f705d42f1e 497 rpy[YAW] += 360.0f;
taknokolat 4:67f705d42f1e 498 rpy[YAW] -= FirstYAW;
taknokolat 4:67f705d42f1e 499 }
taknokolat 4:67f705d42f1e 500 }
Skykon 3:c18342e4fddd 501
Skykon 3:c18342e4fddd 502 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 503 if(!flg_checkoutlier || count_changeRPY >= 2){
Skykon 3:c18342e4fddd 504 for(uint8_t i=0; i<3; i++){
Skykon 3:c18342e4fddd 505 nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f; //2つの移動平均
Skykon 3:c18342e4fddd 506 }
Skykon 3:c18342e4fddd 507 count_changeRPY = 0;
Skykon 3:c18342e4fddd 508 }else count_changeRPY++;
Skykon 3:c18342e4fddd 509 flg_checkoutlier = false;
Skykon 3:c18342e4fddd 510
Skykon 3:c18342e4fddd 511 }
Skykon 3:c18342e4fddd 512
Skykon 3:c18342e4fddd 513
Skykon 3:c18342e4fddd 514 void Init_sensors(){
Skykon 3:c18342e4fddd 515 if(mpu6050.setup() == -1){
Skykon 3:c18342e4fddd 516 pc.printf("failed initialize\r\n");
Skykon 3:c18342e4fddd 517 }
Skykon 3:c18342e4fddd 518 }
Skykon 3:c18342e4fddd 519
Skykon 3:c18342e4fddd 520
Skykon 3:c18342e4fddd 521 void DisplayClock(){
Skykon 3:c18342e4fddd 522 pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000);
Skykon 3:c18342e4fddd 523 pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000);
Skykon 3:c18342e4fddd 524 pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000);
Skykon 3:c18342e4fddd 525 pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000);
Skykon 3:c18342e4fddd 526 pc.printf("\r\n");
taknokolat 4:67f705d42f1e 527 }
taknokolat 6:166746820555 528
taknokolat 6:166746820555 529 void SensingHMC(){
taknokolat 6:166746820555 530 //static int16_t deltaT = 0, t_start = 0;
taknokolat 6:166746820555 531 //t_start = t.read_us();
taknokolat 6:166746820555 532
taknokolat 7:8989a4b84695 533 float rpy=0;
taknokolat 6:166746820555 534 static uint16_t count_changeRPY = 0;
taknokolat 6:166746820555 535 static bool flg_checkoutlier = false;
taknokolat 6:166746820555 536 NVIC_DisableIRQ(USART1_IRQn);
taknokolat 6:166746820555 537 NVIC_DisableIRQ(USART2_IRQn);
taknokolat 6:166746820555 538 NVIC_DisableIRQ(TIM5_IRQn);
taknokolat 6:166746820555 539 NVIC_DisableIRQ(EXTI0_IRQn);
taknokolat 6:166746820555 540 NVIC_DisableIRQ(EXTI9_5_IRQn);
taknokolat 4:67f705d42f1e 541
taknokolat 6:166746820555 542 rpy= compass.getHeadingXYDeg(20,50);
taknokolat 6:166746820555 543
taknokolat 6:166746820555 544 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 6:166746820555 545 NVIC_EnableIRQ(USART2_IRQn);
taknokolat 6:166746820555 546 NVIC_EnableIRQ(TIM5_IRQn);
taknokolat 6:166746820555 547 NVIC_EnableIRQ(EXTI0_IRQn);
taknokolat 6:166746820555 548 NVIC_EnableIRQ(EXTI9_5_IRQn);
taknokolat 6:166746820555 549
taknokolat 6:166746820555 550
taknokolat 6:166746820555 551 //外れ値対策
taknokolat 6:166746820555 552 //rpy*= 180.0f/PI;
taknokolat 6:166746820555 553 if(!setupFlag){
taknokolat 6:166746820555 554 rpy -= g_FirstYAW_HMC;
taknokolat 6:166746820555 555 }else{
taknokolat 6:166746820555 556 if(rpy >= g_FirstYAW_HMC){
taknokolat 6:166746820555 557 rpy -= g_FirstYAW_HMC;
taknokolat 6:166746820555 558 }else{
taknokolat 6:166746820555 559 rpy += 360.0f;
taknokolat 6:166746820555 560 rpy -= g_FirstYAW_HMC;
taknokolat 6:166746820555 561 }
taknokolat 6:166746820555 562 }
taknokolat 6:166746820555 563
taknokolat 6:166746820555 564 if(rpy < nowAngle_HMC-10 || rpy > nowAngle_HMC+10) {flg_checkoutlier = true;}
taknokolat 6:166746820555 565 if(!flg_checkoutlier || count_changeRPY >= 2){
taknokolat 6:166746820555 566
taknokolat 6:166746820555 567 nowAngle_HMC = (rpy + nowAngle_HMC)/2.0f; //2つの移動平均
taknokolat 6:166746820555 568
taknokolat 6:166746820555 569 count_changeRPY = 0;
taknokolat 6:166746820555 570 }else count_changeRPY++;
taknokolat 6:166746820555 571 flg_checkoutlier = false;
taknokolat 6:166746820555 572
taknokolat 6:166746820555 573 }
taknokolat 12:083662bca47d 574
taknokolat 12:083662bca47d 575 int SetOptions(int *Servo_NEUTRAL_R, int *Servo_NEUTRAL_L,
taknokolat 12:083662bca47d 576 int *Servo_high_FORWARD_R, int *Servo_high_FORWARD_L,
taknokolat 12:083662bca47d 577 int *Servo_slow_FORWARD_R,int *Servo_slow_FORWARD_L,
taknokolat 12:083662bca47d 578 int *Turntable_NEUTRAL,int *Matchspeed,int *Focus_NEUTRAL,
taknokolat 12:083662bca47d 579 int *Camera_deg_A, int *Camera_deg_B
taknokolat 12:083662bca47d 580 ){
taknokolat 12:083662bca47d 581
taknokolat 12:083662bca47d 582 pc.printf("SDsetup start.\r\n");
taknokolat 12:083662bca47d 583
taknokolat 12:083662bca47d 584 FILE *fp;
taknokolat 12:083662bca47d 585 char parameter[20]; //文字列渡す用の配列
taknokolat 12:083662bca47d 586 int SDerrorcount = 0; //取得できなかった数を返す
taknokolat 12:083662bca47d 587 const char *paramNames[] = {
taknokolat 12:083662bca47d 588 "SERVO_NEUTRAL_R",
taknokolat 12:083662bca47d 589 "SERVO_NEUTRAL_L",
taknokolat 12:083662bca47d 590 "SERVO_HIGH_FORWARD_R",
taknokolat 12:083662bca47d 591 "SERVO_HIGH_FORWARD_L",
taknokolat 12:083662bca47d 592 "SERVO_SLOW_FORWARD_R",
taknokolat 12:083662bca47d 593 "SERVO_SLOW_FORWARD_L",
taknokolat 12:083662bca47d 594 "TURNTABLE_NEUTRAL",
taknokolat 12:083662bca47d 595 "MATCH_SPEED",
taknokolat 12:083662bca47d 596 "FOCUS_NEUTRAL",
taknokolat 12:083662bca47d 597 "CAMERA_DEG_A",
taknokolat 12:083662bca47d 598 "CAMERA_DEG_B"
taknokolat 12:083662bca47d 599 };
taknokolat 12:083662bca47d 600
taknokolat 12:083662bca47d 601 fp = fopen("/sd/option.txt","r");
taknokolat 12:083662bca47d 602
taknokolat 12:083662bca47d 603 if(fp != NULL){ //開けたら
taknokolat 12:083662bca47d 604 pc.printf("File was openned.\r\n");
taknokolat 12:083662bca47d 605 if(GetParameter(fp,paramNames[0],parameter)) *Servo_NEUTRAL_R = atof(parameter);
taknokolat 12:083662bca47d 606 else{ *Servo_NEUTRAL_R = servo_NEUTRAL_R;
taknokolat 12:083662bca47d 607 SDerrorcount++;
taknokolat 12:083662bca47d 608 }
taknokolat 12:083662bca47d 609 if(GetParameter(fp,paramNames[1],parameter)) *Servo_NEUTRAL_L = atof(parameter);
taknokolat 12:083662bca47d 610 else{ *Servo_NEUTRAL_L = servo_NEUTRAL_L;
taknokolat 12:083662bca47d 611 SDerrorcount++;
taknokolat 12:083662bca47d 612 }
taknokolat 12:083662bca47d 613 if(GetParameter(fp,paramNames[2],parameter)) *Servo_high_FORWARD_R = atof(parameter);
taknokolat 12:083662bca47d 614 else{ *Servo_high_FORWARD_R = servo_high_FORWARD_R;
taknokolat 12:083662bca47d 615 SDerrorcount++;
taknokolat 12:083662bca47d 616 }
taknokolat 12:083662bca47d 617 if(GetParameter(fp,paramNames[3],parameter)) *Servo_high_FORWARD_L = atof(parameter);
taknokolat 12:083662bca47d 618 else{ *Servo_high_FORWARD_L = servo_high_FORWARD_L;
taknokolat 12:083662bca47d 619 SDerrorcount++;
taknokolat 12:083662bca47d 620 }
taknokolat 12:083662bca47d 621 if(GetParameter(fp,paramNames[4],parameter)) *Servo_slow_FORWARD_R = atof(parameter);
taknokolat 12:083662bca47d 622 else{ *Servo_slow_FORWARD_R = servo_slow_FORWARD_R;
taknokolat 12:083662bca47d 623 SDerrorcount++;
taknokolat 12:083662bca47d 624 }
taknokolat 12:083662bca47d 625 if(GetParameter(fp,paramNames[5],parameter)) *Servo_slow_FORWARD_L = atof(parameter);
taknokolat 12:083662bca47d 626 else{ *Servo_slow_FORWARD_L = servo_slow_FORWARD_L;
taknokolat 12:083662bca47d 627 SDerrorcount++;
taknokolat 12:083662bca47d 628 }
taknokolat 12:083662bca47d 629
taknokolat 12:083662bca47d 630 if(GetParameter(fp,paramNames[6],parameter)) *Turntable_NEUTRAL = atof(parameter);
taknokolat 12:083662bca47d 631 else{ *Turntable_NEUTRAL = turntable_NEUTRAL;
taknokolat 12:083662bca47d 632 SDerrorcount++;
taknokolat 12:083662bca47d 633 }
taknokolat 12:083662bca47d 634 if(GetParameter(fp,paramNames[7],parameter)) *Matchspeed = atof(parameter);
taknokolat 12:083662bca47d 635 else{ *Matchspeed = matchspeed;
taknokolat 12:083662bca47d 636 SDerrorcount++;
taknokolat 12:083662bca47d 637 }
taknokolat 12:083662bca47d 638 if(GetParameter(fp,paramNames[8],parameter)) *Focus_NEUTRAL = atof(parameter);
taknokolat 12:083662bca47d 639 else{ *Focus_NEUTRAL = focus_NEUTRAL;
taknokolat 12:083662bca47d 640 SDerrorcount++;
taknokolat 12:083662bca47d 641 }
taknokolat 12:083662bca47d 642 if(GetParameter(fp,paramNames[9],parameter)) *Camera_deg_A = atof(parameter);
taknokolat 12:083662bca47d 643 else{ *Camera_deg_A = camera_deg_A;
taknokolat 12:083662bca47d 644 SDerrorcount++;
taknokolat 12:083662bca47d 645 }
taknokolat 12:083662bca47d 646 if(GetParameter(fp,paramNames[10],parameter)) *Camera_deg_B = atof(parameter);
taknokolat 12:083662bca47d 647 else{ *Camera_deg_B = camera_deg_B;
taknokolat 12:083662bca47d 648 SDerrorcount++;
taknokolat 12:083662bca47d 649 }
taknokolat 12:083662bca47d 650
taknokolat 12:083662bca47d 651 fclose(fp);
taknokolat 12:083662bca47d 652
taknokolat 12:083662bca47d 653 }else{ //ファイルがなかったら
taknokolat 12:083662bca47d 654 pc.printf("fp was null.\r\n");
taknokolat 12:083662bca47d 655
taknokolat 12:083662bca47d 656 *Servo_NEUTRAL_R = servo_NEUTRAL_R;
taknokolat 12:083662bca47d 657 *Servo_NEUTRAL_L = servo_NEUTRAL_L;
taknokolat 12:083662bca47d 658 *Servo_high_FORWARD_R = servo_high_FORWARD_R;
taknokolat 12:083662bca47d 659 *Servo_high_FORWARD_L = servo_high_FORWARD_L;
taknokolat 12:083662bca47d 660 *Servo_slow_FORWARD_R = servo_slow_FORWARD_R;
taknokolat 12:083662bca47d 661 *Servo_slow_FORWARD_L = servo_slow_FORWARD_L;
taknokolat 12:083662bca47d 662 *Turntable_NEUTRAL = turntable_NEUTRAL;
taknokolat 12:083662bca47d 663 *Matchspeed = matchspeed;
taknokolat 12:083662bca47d 664 *Focus_NEUTRAL = focus_NEUTRAL;
taknokolat 12:083662bca47d 665 *Camera_deg_A = camera_deg_A;
taknokolat 12:083662bca47d 666 *Camera_deg_B = camera_deg_B;
taknokolat 12:083662bca47d 667 SDerrorcount = -1;
taknokolat 12:083662bca47d 668 }
taknokolat 12:083662bca47d 669 pc.printf("SDsetup finished.\r\n");
taknokolat 12:083662bca47d 670 if(SDerrorcount == 0) pc.printf("setting option is success\r\n");
taknokolat 12:083662bca47d 671 else if(SDerrorcount == -1) pc.printf("ERROR 1. cannot open option\r\n");
taknokolat 12:083662bca47d 672 else if(SDerrorcount > 0) pc.printf("ERROR 2. reading parameter is failed[%d]\r\n",SDerrorcount);
taknokolat 12:083662bca47d 673
taknokolat 12:083662bca47d 674 pc.printf("Servo_NEUTRAL_R = %d, Servo_NEUTRAL_L = %d\r\n", *Servo_NEUTRAL_R, *Servo_NEUTRAL_L);
taknokolat 13:b088f0db7158 675 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 676 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 677 pc.printf("Turntable_NEUTRAL = %d, Matchspeed = %d\r\n", *Turntable_NEUTRAL, *Matchspeed);
taknokolat 13:b088f0db7158 678 pc.printf("Focus_NEUTRAL = %d\r\n", *Focus_NEUTRAL);
taknokolat 13:b088f0db7158 679 pc.printf("Camera_deg_A = %d, Camera_deg_B = %d\r\n", *Camera_deg_A, *Camera_deg_B);
taknokolat 12:083662bca47d 680
taknokolat 12:083662bca47d 681 return SDerrorcount;
taknokolat 12:083662bca47d 682 }
taknokolat 12:083662bca47d 683
taknokolat 12:083662bca47d 684 int GetParameter(FILE *fp, const char *paramName,char parameter[]){
taknokolat 12:083662bca47d 685 int i=0, j=0;
taknokolat 12:083662bca47d 686 int strmax = 200;
taknokolat 12:083662bca47d 687 char str[strmax];
taknokolat 12:083662bca47d 688
taknokolat 12:083662bca47d 689 rewind(fp); //ファイル位置を先頭に
taknokolat 12:083662bca47d 690 while(1){
taknokolat 12:083662bca47d 691 if (fgets(str, strmax, fp) == NULL) {
taknokolat 12:083662bca47d 692 return 0;
taknokolat 12:083662bca47d 693 }
taknokolat 12:083662bca47d 694 if (!strncmp(str, paramName, strlen(paramName))) {
taknokolat 12:083662bca47d 695 while (str[i++] != '=') {}
taknokolat 12:083662bca47d 696 while (str[i] != '\n') {
taknokolat 12:083662bca47d 697 parameter[j++] = str[i++];
taknokolat 12:083662bca47d 698 }
taknokolat 12:083662bca47d 699 parameter[j] = '\0';
taknokolat 12:083662bca47d 700 return 1;
taknokolat 12:083662bca47d 701 }
taknokolat 12:083662bca47d 702 }
taknokolat 12:083662bca47d 703 }
taknokolat 12:083662bca47d 704
taknokolat 12:083662bca47d 705
taknokolat 6:166746820555 706
taknokolat 4:67f705d42f1e 707 void DebugPrint(){
taknokolat 4:67f705d42f1e 708 //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); //skipper地磁気センサ_デバック用
taknokolat 7:8989a4b84695 709 //pc.printf("%3.2f\t",nowAngle[2]);
taknokolat 7:8989a4b84695 710 //pc.printf("%3.2f\t",nowAngle_HMC); //HMC
taknokolat 7:8989a4b84695 711 //pc.printf("\r\n");
taknokolat 6:166746820555 712 }