航空研究会 / Mbed 2 deprecated hassi_18

SDの設定が反映されるように修正

Dependencies:   mbed MPU6050_2 HMC5883L_2 SDFileSystem3

main.cpp@2:f30666d7838b, 2018-12-24 (annotated)

Committer:
taknokolat
Date:
Mon Dec 24 07:47:22 2018 +0000
Revision:
2:f30666d7838b
Parent:
1:290e621741fd
Child:
3:c18342e4fddd
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
HARUKIDELTA 0:84ddd6d354e1 1 #include "mbed.h"
HARUKIDELTA 0:84ddd6d354e1 2
HARUKIDELTA 0:84ddd6d354e1 3 #define servo_NEUTRAL_R 1460
HARUKIDELTA 0:84ddd6d354e1 4 #define servo_NEUTRAL_L 1460
HARUKIDELTA 0:84ddd6d354e1 5 #define servo_FORWARD_R 1860
HARUKIDELTA 0:84ddd6d354e1 6 #define servo_FORWARD_L 1860
HARUKIDELTA 0:84ddd6d354e1 7 #define servo_back_R 1060
HARUKIDELTA 0:84ddd6d354e1 8 #define servo_back_L 1060
HARUKIDELTA 0:84ddd6d354e1 9 #define servo_slow_FORWARD_R 1560
HARUKIDELTA 0:84ddd6d354e1 10 #define servo_slow_FORWARD_L 1560
HARUKIDELTA 0:84ddd6d354e1 11 #define servo_slow_back_R 1360
HARUKIDELTA 0:84ddd6d354e1 12 #define servo_slow_back_L 1360
HARUKIDELTA 0:84ddd6d354e1 13
HARUKIDELTA 0:84ddd6d354e1 14 #define MOVE_NEUTRAL 0
HARUKIDELTA 0:84ddd6d354e1 15 #define MOVE_FORWARD 1
HARUKIDELTA 0:84ddd6d354e1 16 #define MOVE_LEFT 2
HARUKIDELTA 0:84ddd6d354e1 17 #define MOVE_RIGHT 3
HARUKIDELTA 0:84ddd6d354e1 18 #define MOVE_BACK 4
HARUKIDELTA 0:84ddd6d354e1 19 #define GOAL_FORWARD 5 //ゴール付近 ゆっくり
HARUKIDELTA 0:84ddd6d354e1 20 #define GOAL_LEFT 6
HARUKIDELTA 0:84ddd6d354e1 21 #define GOAL_RIGHT 7
HARUKIDELTA 0:84ddd6d354e1 22 #define MAX_FORWARD 8 //はやい 姿勢修正用
HARUKIDELTA 0:84ddd6d354e1 23 #define MAX_BACK 9
HARUKIDELTA 0:84ddd6d354e1 24
taknokolat 1:290e621741fd 25 void getSF_Serial_jevois();
taknokolat 1:290e621741fd 26 void getSF_Serial_pi();
taknokolat 1:290e621741fd 27
HARUKIDELTA 0:84ddd6d354e1 28 PwmOut servoR(PC_6);
HARUKIDELTA 0:84ddd6d354e1 29 PwmOut servoL(PC_7);
HARUKIDELTA 0:84ddd6d354e1 30
taknokolat 2:f30666d7838b 31 RawSerial pc(PA_2,PA_3,115200); //uart2
taknokolat 2:f30666d7838b 32 RawSerial pc2(PB_6,PB_7,115200); //uart1
taknokolat 1:290e621741fd 33
taknokolat 2:f30666d7838b 34 char g_landingcommand='N';
taknokolat 1:290e621741fd 35
taknokolat 1:290e621741fd 36 void MoveCansat(char g_landingcommand);
HARUKIDELTA 0:84ddd6d354e1 37
HARUKIDELTA 0:84ddd6d354e1 38 int main() {
taknokolat 1:290e621741fd 39
taknokolat 1:290e621741fd 40 // シリアル通信受信の割り込みイベント登録
taknokolat 1:290e621741fd 41 pc.attach(getSF_Serial_jevois, Serial::RxIrq);
taknokolat 1:290e621741fd 42 pc2.attach(getSF_Serial_pi, Serial::RxIrq);
taknokolat 1:290e621741fd 43
taknokolat 1:290e621741fd 44 NVIC_SetPriority(USART1_IRQn,0);//割り込み優先度
taknokolat 1:290e621741fd 45 NVIC_SetPriority(USART2_IRQn,1);
taknokolat 1:290e621741fd 46
HARUKIDELTA 0:84ddd6d354e1 47 while(1) {
taknokolat 1:290e621741fd 48 //pc.printf("Hello World!!");
taknokolat 1:290e621741fd 49 MoveCansat(g_landingcommand);
HARUKIDELTA 0:84ddd6d354e1 50 }
HARUKIDELTA 0:84ddd6d354e1 51 }
HARUKIDELTA 0:84ddd6d354e1 52
taknokolat 1:290e621741fd 53 void MoveCansat(char g_landingcommand)
HARUKIDELTA 0:84ddd6d354e1 54 {
taknokolat 2:f30666d7838b 55 //NVIC_DisableIRQ(USART1_IRQn);
taknokolat 1:290e621741fd 56 NVIC_DisableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 57 switch(g_landingcommand){
taknokolat 2:f30666d7838b 58 case 'N': //MOVE_NEUTRAL
HARUKIDELTA 0:84ddd6d354e1 59 servoR.pulsewidth_us(servo_NEUTRAL_R);
HARUKIDELTA 0:84ddd6d354e1 60 servoL.pulsewidth_us(servo_NEUTRAL_L);
taknokolat 2:f30666d7838b 61 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 2:f30666d7838b 62 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 63 break;
taknokolat 1:290e621741fd 64
taknokolat 2:f30666d7838b 65 case 'Y': //MOVE_FORWARD
HARUKIDELTA 0:84ddd6d354e1 66 servoR.pulsewidth_us(servo_FORWARD_R);
HARUKIDELTA 0:84ddd6d354e1 67 servoL.pulsewidth_us(servo_FORWARD_L);
taknokolat 2:f30666d7838b 68 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 2:f30666d7838b 69 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 70 break;
taknokolat 1:290e621741fd 71
taknokolat 1:290e621741fd 72 case 'L': //MOVE_LEFT
HARUKIDELTA 0:84ddd6d354e1 73 servoR.pulsewidth_us(servo_slow_FORWARD_R);
HARUKIDELTA 0:84ddd6d354e1 74 servoL.pulsewidth_us(servo_slow_back_L);
taknokolat 2:f30666d7838b 75 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 2:f30666d7838b 76 NVIC_EnableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 77
taknokolat 2:f30666d7838b 78 case 'R': //MOVE_RIGHT
HARUKIDELTA 0:84ddd6d354e1 79 servoR.pulsewidth_us(servo_slow_back_R);
HARUKIDELTA 0:84ddd6d354e1 80 servoL.pulsewidth_us(servo_slow_FORWARD_L);
taknokolat 2:f30666d7838b 81 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 2:f30666d7838b 82 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 83 break;
taknokolat 1:290e621741fd 84
taknokolat 1:290e621741fd 85 case 'B': //MOVE_BACK
HARUKIDELTA 0:84ddd6d354e1 86 servoR.pulsewidth_us(servo_back_R);
HARUKIDELTA 0:84ddd6d354e1 87 servoL.pulsewidth_us(servo_back_L);
taknokolat 2:f30666d7838b 88 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 2:f30666d7838b 89 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 90 break;
taknokolat 1:290e621741fd 91
taknokolat 1:290e621741fd 92 case 'G': //GOAL_FORWARD
HARUKIDELTA 0:84ddd6d354e1 93 servoR.pulsewidth_us(servo_slow_FORWARD_R);
HARUKIDELTA 0:84ddd6d354e1 94 servoL.pulsewidth_us(servo_slow_FORWARD_L);
taknokolat 2:f30666d7838b 95 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 2:f30666d7838b 96 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 97 break;
taknokolat 1:290e621741fd 98
taknokolat 1:290e621741fd 99 default :
taknokolat 2:f30666d7838b 100 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 1:290e621741fd 101 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 102 break;
taknokolat 1:290e621741fd 103
HARUKIDELTA 0:84ddd6d354e1 104 }
HARUKIDELTA 0:84ddd6d354e1 105 return;
taknokolat 1:290e621741fd 106 }
taknokolat 1:290e621741fd 107
taknokolat 1:290e621741fd 108 void getSF_Serial_jevois(){
taknokolat 1:290e621741fd 109
taknokolat 1:290e621741fd 110
taknokolat 1:290e621741fd 111 static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
taknokolat 1:290e621741fd 112
taknokolat 1:290e621741fd 113 static int bufcounter=0;
taknokolat 1:290e621741fd 114
taknokolat 1:290e621741fd 115
taknokolat 1:290e621741fd 116
taknokolat 1:290e621741fd 117 if(pc.readable()) { // 受信確認
taknokolat 1:290e621741fd 118
taknokolat 1:290e621741fd 119 SFbuf[bufcounter] = pc.getc(); // 1文字取り出し
taknokolat 1:290e621741fd 120 if(SFbuf[0]!='S'){
taknokolat 1:290e621741fd 121 //pc.printf("x");
taknokolat 1:290e621741fd 122 return;
taknokolat 1:290e621741fd 123 }
taknokolat 1:290e621741fd 124
taknokolat 1:290e621741fd 125
taknokolat 1:290e621741fd 126
taknokolat 1:290e621741fd 127 //pc.printf("%c",SFbuf[bufcounter]);
taknokolat 1:290e621741fd 128
taknokolat 1:290e621741fd 129 if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++;
taknokolat 1:290e621741fd 130
taknokolat 1:290e621741fd 131 if(bufcounter==5 && SFbuf[4]=='F'){
taknokolat 1:290e621741fd 132
taknokolat 1:290e621741fd 133 g_landingcommand = SFbuf[1];
taknokolat 2:f30666d7838b 134 wait_ms(31);//信号が速すぎることによる割り込み防止
taknokolat 1:290e621741fd 135 //pc.printf("%c",g_landingcommand);
taknokolat 1:290e621741fd 136 //wait_ms(20);
taknokolat 1:290e621741fd 137 //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
taknokolat 1:290e621741fd 138 bufcounter = 0;
taknokolat 1:290e621741fd 139 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 140 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 141 //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
taknokolat 1:290e621741fd 142 }
taknokolat 1:290e621741fd 143
taknokolat 1:290e621741fd 144 else if(bufcounter>=5){
taknokolat 1:290e621741fd 145 //pc.printf("Communication Falsed.\r\n");
taknokolat 1:290e621741fd 146 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 147 bufcounter = 0;
taknokolat 1:290e621741fd 148 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 149 }
taknokolat 1:290e621741fd 150 }
taknokolat 1:290e621741fd 151
taknokolat 1:290e621741fd 152
taknokolat 1:290e621741fd 153 }
taknokolat 1:290e621741fd 154
taknokolat 1:290e621741fd 155
taknokolat 1:290e621741fd 156 void getSF_Serial_pi(){
taknokolat 2:f30666d7838b 157
taknokolat 2:f30666d7838b 158 NVIC_DisableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 159
taknokolat 1:290e621741fd 160 static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
taknokolat 1:290e621741fd 161
taknokolat 1:290e621741fd 162 static int bufcounter=0;
taknokolat 1:290e621741fd 163
taknokolat 1:290e621741fd 164
taknokolat 1:290e621741fd 165
taknokolat 1:290e621741fd 166 if(pc2.readable()) { // 受信確認
taknokolat 1:290e621741fd 167
taknokolat 1:290e621741fd 168 SFbuf[bufcounter] = pc2.getc(); // 1文字取り出し
taknokolat 1:290e621741fd 169 if(SFbuf[0]!='S'){
taknokolat 1:290e621741fd 170 //pc.printf("x");
taknokolat 1:290e621741fd 171 return;
taknokolat 1:290e621741fd 172 }
taknokolat 1:290e621741fd 173
taknokolat 1:290e621741fd 174
taknokolat 1:290e621741fd 175
taknokolat 1:290e621741fd 176 //pc.printf("%c",SFbuf[bufcounter]);
taknokolat 1:290e621741fd 177
taknokolat 1:290e621741fd 178 if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++;
taknokolat 1:290e621741fd 179
taknokolat 1:290e621741fd 180 if(bufcounter==5 && SFbuf[4]=='F'){
taknokolat 1:290e621741fd 181
taknokolat 1:290e621741fd 182 g_landingcommand = SFbuf[1];
taknokolat 2:f30666d7838b 183 wait_ms(31);//信号が速すぎることによる割り込み防止
taknokolat 1:290e621741fd 184 //pc.printf("%c",g_landingcommand);
taknokolat 1:290e621741fd 185 //wait_ms(20);
taknokolat 1:290e621741fd 186 //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
taknokolat 1:290e621741fd 187 bufcounter = 0;
taknokolat 1:290e621741fd 188 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 189 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 190 //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
taknokolat 1:290e621741fd 191 }
taknokolat 1:290e621741fd 192
taknokolat 1:290e621741fd 193 else if(bufcounter>=5){
taknokolat 1:290e621741fd 194 //pc.printf("Communication Falsed.\r\n");
taknokolat 1:290e621741fd 195 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 196 bufcounter = 0;
taknokolat 1:290e621741fd 197 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 198 }
taknokolat 1:290e621741fd 199 }
taknokolat 2:f30666d7838b 200
taknokolat 2:f30666d7838b 201 NVIC_EnableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 202
taknokolat 1:290e621741fd 203
taknokolat 1:290e621741fd 204 }
taknokolat 1:290e621741fd 205