Skip to content
This repository was archived by the owner on Feb 3, 2025. It is now read-only.

Commit d457aa3

Browse files
Yuzz1eYuzz1e
committed
🎉 Abillity to play tone in remote,IMU rst fix
1 parent f9655f8 commit d457aa3

2 files changed

Lines changed: 171 additions & 44 deletions

File tree

‎RACOON-Pi‎

184 KB
Binary file not shown.

‎main.go‎

Lines changed: 171 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,8 @@ import (
1010
"os"
1111
"os/exec"
1212
"os/signal"
13+
"strconv"
14+
"strings"
1315
"time"
1416

1517
"github.com/Rione-SSL/RACOON-Pi/proto/pb_gen"
@@ -35,7 +37,7 @@ const BALLSENS_LOW_THRESHOULD int = 100
3537

3638
// バッテリーの低下しきい値。 150 = 15.0V
3739
const BATTERY_LOW_THRESHOULD int = 150
38-
const BATTERY_CRITICAL_THRESHOULD int = 140
40+
const BATTERY_CRITICAL_THRESHOULD int = 145
3941

4042
var sendarray bytes.Buffer //送信用バッファ
4143

@@ -71,7 +73,7 @@ var imuError bool = false
7173
var last_recv_time time.Time = time.Now()
7274

7375
// ポート8080番で待ち受ける。
74-
const PORT string = ":8080"
76+
const PORT string = ":9191"
7577

7678
func RunApi(chapi chan bool, MyID uint32) {
7779
//ポートを開く
@@ -87,6 +89,8 @@ func RunApi(chapi chan bool, MyID uint32) {
8789
if err != nil {
8890
log.Fatal(err)
8991
}
92+
// log
93+
log.Println("Remote API Connected by ", conn.RemoteAddr())
9094
//接続があったら処理を行う
9195
go HandleRequest(conn)
9296
}
@@ -100,27 +104,119 @@ var RobotErrorMessage = ""
100104

101105
var ballSensLowCount = 0
102106

107+
var doBuzzer = false
108+
var buzzerTone = 0
109+
var buzzerTime time.Duration = 0 * time.Millisecond
110+
111+
var alarmIgnore = false
112+
103113
// 接続があったら処理を行う
104114
func HandleRequest(conn net.Conn) {
105-
//200 OKを返す
106-
fmt.Fprintf(conn, "HTTP/1.1 200 OK\r \n")
107-
108-
//そのまま、recvdataをJson形式で返す
109-
//fmt.Fprintf(conn, "%s", recvdata)
110-
111-
//Json形式で返す
112-
fmt.Fprintf(conn, "{")
113-
fmt.Fprintf(conn, "\"VOLT\": %f,", float32(recvdata.Volt)/10.0)
114-
fmt.Fprintf(conn, "\"PHOTOSENSOR\":%d,", recvdata.PhotoSensor)
115-
fmt.Fprintf(conn, "\"ISHOLDBALL\":%t,", recvdata.IsHoldBall)
116-
fmt.Fprintf(conn, "\"IMUDIR\":%d", recvdata.ImuDir)
117-
fmt.Fprintf(conn, "\"ERROR \":%t", isRobotError)
118-
fmt.Fprintf(conn, "\"ERRORCODE \":%d", RobotErrorCode)
119-
fmt.Fprintf(conn, "\"ERRORMESSAGE \":%s", RobotErrorMessage)
120-
fmt.Fprintf(conn, "}")
121-
122-
//接続を閉じる
123-
conn.Close()
115+
defer conn.Close()
116+
117+
//リクエストを解析
118+
buf := make([]byte, 1024)
119+
_, err := conn.Read(buf)
120+
if err != nil {
121+
log.Println(err)
122+
return
123+
}
124+
125+
// リクエストを解析
126+
// リクエストヘッダーの1行目を取得
127+
request := string(buf)
128+
// リクエストヘッダーの1行目をスペースで区切る
129+
requests := strings.Split(request, " ")
130+
// リクエストヘッダーの1行目からリクエストの種類を取得
131+
requestType := requests[0]
132+
133+
// リクエストの種類がGETでなければエラーを返す
134+
if requestType != "GET" {
135+
fmt.Fprintf(conn, "HTTP/1.1 400 Bad Request\r\n")
136+
fmt.Fprintf(conn, "Content-Type: text/plain; charset=utf-8\r\n\r\n")
137+
fmt.Fprintf(conn, "400 Bad Request\r\n")
138+
return
139+
}
140+
141+
// リクエストのパスが"/buzzer"の場合
142+
if strings.Split(requests[1], "/")[1] == "buzzer" {
143+
// tone が指定されていない場合
144+
if len(requests) < 3 {
145+
fmt.Fprintf(conn, "HTTP/1.1 400 Bad Request\r\n")
146+
fmt.Fprintf(conn, "Content-Type: text/plain; charset=utf-8\r\n\r\n")
147+
fmt.Fprintf(conn, "400 Bad Request\r\n")
148+
return
149+
}
150+
// buzzer/ の後に tone が指定されている場合
151+
log.Println(strings.Split(requests[1], "/")[1])
152+
tone, err := strconv.Atoi(strings.Split(requests[1], "/")[3])
153+
if err != nil {
154+
fmt.Fprintf(conn, "HTTP/1.1 400 Bad Request\r\n")
155+
fmt.Fprintf(conn, "Content-Type: text/plain; charset=utf-8\r\n\r\n")
156+
fmt.Fprintf(conn, "400 Bad Request\r\n")
157+
return
158+
}
159+
duration, err := strconv.Atoi(strings.Split(requests[1], "/")[4])
160+
if err != nil {
161+
fmt.Fprintf(conn, "HTTP/1.1 400 Bad Request\r\n")
162+
fmt.Fprintf(conn, "Content-Type: text/plain; charset=utf-8\r\n\r\n")
163+
fmt.Fprintf(conn, "400 Bad Request\r\n")
164+
return
165+
}
166+
// tone が 0 から 12 でない場合
167+
if tone < 0 || tone > 15 {
168+
fmt.Fprintf(conn, "HTTP/1.1 400 Bad Request\r\n")
169+
fmt.Fprintf(conn, "Content-Type: text/plain; charset=utf-8\r\n\r\n")
170+
fmt.Fprintf(conn, "400 Bad Request\r\n")
171+
return
172+
}
173+
// duration が 50 から 3000 でない場合
174+
if duration < 50 || duration > 3000 {
175+
fmt.Fprintf(conn, "HTTP/1.1 400 Bad Request\r\n")
176+
fmt.Fprintf(conn, "Content-Type: text/plain; charset=utf-8\r\n\r\n")
177+
fmt.Fprintf(conn, "400 Bad Request\r\n")
178+
return
179+
}
180+
181+
// OK と表示
182+
fmt.Fprintf(conn, "HTTP/1.1 200 OK\r\n")
183+
fmt.Fprintf(conn, "Content-Type: text/plain; charset=utf-8\r\n\r\n")
184+
fmt.Fprintf(conn, "BUZZER OK\r\n")
185+
//ブザーを1秒鳴らす
186+
doBuzzer = true
187+
buzzerTone = tone
188+
buzzerTime = time.Duration(duration) * time.Millisecond
189+
return
190+
}
191+
192+
if strings.Split(requests[1], "/")[1] == "ignorebatterylow" {
193+
// OK と表示
194+
fmt.Fprintf(conn, "HTTP/1.1 200 OK\r\n")
195+
fmt.Fprintf(conn, "Content-Type: text/plain; charset=utf-8\r\n\r\n")
196+
fmt.Fprintf(conn, "IGNORE BATTERY LOW OK\r\n")
197+
//アラーム無視をセットする
198+
alarmIgnore = true
199+
return
200+
}
201+
202+
// 200 OKを返す
203+
fmt.Fprintf(conn, "HTTP/1.1 200 OK\r\n")
204+
// UTF-8指定
205+
fmt.Fprintf(conn, "Content-Type: application/json; charset=utf-8\r\n\r\n")
206+
207+
// JSON形式で返す
208+
response := fmt.Sprintf(`{
209+
"VOLT": %f,
210+
"PHOTOSENSOR": %d,
211+
"ISHOLDBALL": %t,
212+
"IMUDIR": %d,
213+
"ERROR": %t,
214+
"ERRORCODE": %d,
215+
"ERRORMESSAGE": "%s"
216+
}`, float32(recvdata.Volt)/10.0, recvdata.PhotoSensor, recvdata.IsHoldBall, recvdata.ImuDir, isRobotError, RobotErrorCode, RobotErrorMessage)
217+
218+
fmt.Fprint(conn, response)
219+
124220
}
125221

126222
// シリアル通信部分
@@ -196,12 +292,12 @@ func RunSerial(chclient chan bool, MyID uint32) {
196292
RobotErrorMessage = "ボールセンサ異常"
197293
}
198294

199-
//ボールセンサの値が極端に高いときはエラー
200-
if recvdata.PhotoSensor > uint16(BALLSENS_HBREAK_THRESHOULD) {
201-
isRobotError = true
202-
RobotErrorCode = 1
203-
RobotErrorMessage = "ボールセンサ異常(回路故障の可能性)"
204-
}
295+
// //ボールセンサの値が極端に高いときはエラー
296+
// if recvdata.PhotoSensor > uint16(BALLSENS_HBREAK_THRESHOULD) {
297+
// isRobotError = true
298+
// RobotErrorCode = 1
299+
// RobotErrorMessage = "ボールセンサ異常(回路故障の可能性)"
300+
// }
205301

206302
//ボールセンサの値が極端に低いときはエラー
207303
if recvdata.PhotoSensor < uint16(BALLSENS_LBREAK_THRESHOULD) {
@@ -226,6 +322,10 @@ func RunSerial(chclient chan bool, MyID uint32) {
226322
isRobotEmgError = true //緊急停止
227323
}
228324

325+
//TODO: Pendingをfalseにした瞬間にAIが受信し始める。Mutex必要か？
326+
if imuResetPending {
327+
imuResetPending = false
328+
}
229329
//クライアントで受け取ったデータをバイト列に変更
230330
sendbytes := sendarray.Bytes()
231331

@@ -236,7 +336,7 @@ func RunSerial(chclient chan bool, MyID uint32) {
236336

237337
//受信しなかった場合に自動的にモーターOFFする
238338
if time.Since(last_recv_time) > 1*time.Second {
239-
log.Println("No Data Recv")
339+
// log.Println("No Data Recv")
240340
for i := 2; i <= 4; i++ {
241341
sendbytes[i] = 100
242342
}
@@ -249,7 +349,7 @@ func RunSerial(chclient chan bool, MyID uint32) {
249349
}
250350

251351
//それぞれのデータを表示
252-
log.Printf("VOLT: %f, BALLSENS: %t, IMUDEG: %d\n", float32(recvdata.Volt)*0.1, recvdata.IsHoldBall, recvdata.ImuDir)
352+
// log.Printf("VOLT: %f, BALLSENS: %t, IMUDEG: %d\n", float32(recvdata.Volt)*0.1, recvdata.IsHoldBall, recvdata.ImuDir)
253353

254354
//高速回転防止機能
255355
//フレームごとの角度が閾値を超えると, EMGをセットする
@@ -283,7 +383,7 @@ func RunSerial(chclient chan bool, MyID uint32) {
283383
port.Write(sendbytes) //書き込み
284384
time.Sleep(16 * time.Millisecond) //少し待つ
285385
//log.Printf("Sent %v bytes\n", n) //何バイト送信した？
286-
log.Println(sendbytes) //送信済みのバイトを表示
386+
// log.Println(sendbytes) //送信済みのバイトを表示
287387

288388
//100ナノ秒待つ
289389
time.Sleep(100 * time.Nanosecond)
@@ -376,22 +476,33 @@ func RunGPIO(chgpio chan bool) {
376476

377477
//Lチカ速度
378478
ledsec := 500 * time.Millisecond
479+
alarmVoltage := BATTERY_LOW_THRESHOULD
379480
for {
380481
//電圧降下検知
381-
if recvdata.Volt <= 155 {
382-
buzzer.Freq(1200 * 64)
383-
buzzer.DutyCycle(16, 32)
482+
if recvdata.Volt <= uint8(alarmVoltage) {
483+
for {
484+
buzzer.Freq(1200 * 64)
485+
buzzer.DutyCycle(16, 32)
486+
487+
//高速チカチカ
488+
led2.High()
489+
time.Sleep(100 * time.Millisecond)
384490

385-
//高速チカチカ
386-
led2.High()
387-
time.Sleep(100 * time.Millisecond)
491+
buzzer.Freq(760 * 64)
492+
led2.Low()
493+
time.Sleep(150 * time.Millisecond)
388494

389-
buzzer.Freq(760 * 64)
390-
led2.Low()
391-
time.Sleep(150 * time.Millisecond)
495+
if button1.Read()^1 == rpio.High || alarmIgnore == true {
496+
//一時的にアラーム解除する
497+
log.Println("BATTERY ALARM IGNORED")
498+
alarmVoltage = BATTERY_CRITICAL_THRESHOULD
499+
break
500+
}
501+
}
392502
} else {
393503
//通常チカチカ。ボタンが押されたら高速チカチカ
394504
//ボタンが押されたら、imuをリセットする
505+
buzzer.Freq(1479 * 64)
395506
time.Sleep(ledsec)
396507
led.Write(rpio.High)
397508
if button1.Read()^1 == rpio.High {
@@ -413,6 +524,14 @@ func RunGPIO(chgpio chan bool) {
413524
time.Sleep(ledsec)
414525
led.Write(rpio.Low)
415526
buzzer.DutyCycle(0, 32)
527+
528+
if doBuzzer {
529+
buzzer.Freq(int(440*math.Pow(1.0595, float64(buzzerTone))) * 64)
530+
buzzer.DutyCycle(16, 32)
531+
time.Sleep(buzzerTime)
532+
buzzer.DutyCycle(0, 32)
533+
doBuzzer = false
534+
}
416535
}
417536
}
418537
}
@@ -596,7 +715,7 @@ func RunServer(chserver chan bool, MyID uint32) {
596715
defer conn.Close()
597716

598717
for {
599-
log.Println(recvdata.IsHoldBall)
718+
// log.Println(recvdata.IsHoldBall)
600719
pe := createStatus(int32(MyID), recvdata.IsHoldBall, false, false)
601720
Data, _ := proto.Marshal(pe)
602721

@@ -607,6 +726,10 @@ func RunServer(chserver chan bool, MyID uint32) {
607726

608727
}
609728

729+
// IMU Resetを確実に行うためのフラグ
730+
// 待機モードにうつり、これがセットされているときはAIから受け取らない
731+
var imuResetPending bool = false
732+
610733
// AIからの情報を受信するクライアント
611734
func RunClient(chclient chan bool, MyID uint32, ip string) {
612735

@@ -750,10 +873,11 @@ func RunClient(chclient chan bool, MyID uint32, ip string) {
750873
//log.Printf("Velnormalized: %f", Velnormalized)
751874
//log.Printf("Float64BeforeInt: %f", Motor)
752875
sendarray = bytes.Buffer{}
753-
err := binary.Write(&sendarray, binary.LittleEndian, bytearray) //バイナリに変換
754-
755-
if err != nil {
756-
log.Fatal(err)
876+
if imuResetPending == false {
877+
err := binary.Write(&sendarray, binary.LittleEndian, bytearray) //バイナリに変換
878+
if err != nil {
879+
log.Fatal(err)
880+
}
757881
}
758882
}
759883
//IDが255のときは、モーター動作させず緊急停止フェーズに移行
@@ -827,6 +951,9 @@ func RunClient(chclient chan bool, MyID uint32, ip string) {
827951

828952
log.Println("=======IMU RESET(RESET TO ANGLE)=======")
829953

954+
//IMU Reset Pending フラグをたてる
955+
imuResetPending = true
956+
830957
if err != nil {
831958
log.Fatal(err)
832959
}

0 commit comments

Comments
 (0)