ケンタ ミヤザキ
/
nhk_auto_06
kkkk
Diff: main.cpp
- Revision:
- 0:158455b60200
- Child:
- 1:681efc14eba9
diff -r 000000000000 -r 158455b60200 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat Jan 19 12:13:21 2019 +0000 @@ -0,0 +1,163 @@ +#include "mbed.h" +#include "speedservo.h" + +//肘 +Servo RFelbow(PA_13); +Servo LFelbow(PA_15); +Servo RBelbow(PC_2); +Servo LBelbow(PC_13); + +//膝 +Servo RFknee(PA_14); +Servo LFknee(PC_1); +Servo RBknee(PC_3); +Servo LBknee(PC_7); + +//肩 +Serial SerialServo(PC_4,PA_10); + +DigitalIn Dmode(PB_13);//歩行方向変更 +DigitalIn start_button(PB_2); + +AnalogIn var(PC_0); +Serial PC(USBTX,USBRX); + +int ser_spe[4] = {32,32,32,32}; +int ser_pos[4] = {135,135,135,135}; +int now_pos[4] = {}; + +int prePos = 2000; +int behPos = 1700; +int upPos = 850; +int downPos = 1350; + +float fmap(float x, float in_min, float in_max, float out_min, float out_max){ + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} + + +void serServoMove(int num,int angle,int speed){ + ser_spe[num] = speed; + ser_pos[num] = angle; +} + +void bluepill_rx(){ + if(SerialServo.readable()){ + int request = SerialServo.getc(); + if(request >= 0 && request <= 3){ + now_pos[request] = SerialServo.getc(); + SerialServo.putc(request); + SerialServo.putc(ser_spe[request]); + SerialServo.putc(ser_pos[request]); + } + } +} +void centerMove(int dev,int num){ + for(int i=0;i<4;i++){ + serServoMove(i,135+dev,30); + } + /* + if(i != num){ + serServoMove(i,135+dev,30); + } + } + */ + +} + +void RFmove(int position,int speed){ + if(2350 <= position){ + position = 2350; + } + RFelbow.SpeedConvert(speed); + RFelbow.SetPosition(fmap(position,850,2350,2350,850)); +} + +void LFmove(int position,int speed){ + if(2350 <= position){ + position = 2350; + } + + LFelbow.SpeedConvert(speed); + LFelbow.SetPosition(position); +} + +void RBmove(int position,int speed){ + if(2350 <= position){ + position = 2350; + } + + RBelbow.SpeedConvert(speed); + RBelbow.SetPosition(position); +} + +void LBmove(int position,int speed){ + if(2350 <= position){ + position = 2350; + } + + LBelbow.SpeedConvert(speed); + LBelbow.SetPosition(fmap(position,850,2350,2350,850)); +} + +void forwarding(){ + RFmove(1600,3); + LFmove(1600,3); + RBmove(2200,3); + LBmove(2200,3); +} + +float angle = 0; +int main() { + //初期設定 + + SerialServo.baud(115200); + SerialServo.attach(bluepill_rx, Serial::RxIrq); + RFelbow.Enable(fmap(behPos,850,2350,2350,850),330,10); + LFelbow.Enable(behPos,330,10); + RBelbow.Enable(prePos,330,10); + LBelbow.Enable(fmap(prePos,850,2350,2350,850),330,10); + + RFknee.Enable(downPos,330,50); + LFknee.Enable(downPos,330,50); + RBknee.Enable(downPos-100,330,50); + LBknee.Enable(downPos-100,330,50); + + serServoMove(0,135,100); + serServoMove(1,135,100); + serServoMove(2,135,100); + serServoMove(3,135,100); + forwarding(); + wait(5); + while (true) { + angle = int(20*var); + PC.printf("%f \n",angle); + centerMove(angle,0); + RBknee.SetPosition(downPos -(var*300)); + LBknee.SetPosition(downPos -(var*300)); + + if(Dmode == true){ + //RFmove(prePos,50); + RFknee.SetPosition(upPos); + wait(2.0); + RFmove(2350,10); + wait(2.0); + RFknee.SetPosition(downPos); + wait(2.0); + RFmove(prePos,25); + wait(2.0); + centerMove(0,0); + RBknee.SetPosition(downPos); + LBknee.SetPosition(downPos); + + RBknee.SetPosition(downPos); + LBknee.SetPosition(downPos); + + }else{ + RFknee.SetPosition(downPos); + forwarding(); + } + + wait(0.1); + } +} \ No newline at end of file