ケンタ ミヤザキ
/
nhk_auto_06
kkkk
Diff: main.cpp
- Revision:
- 1:681efc14eba9
- Parent:
- 0:158455b60200
diff -r 158455b60200 -r 681efc14eba9 main.cpp --- a/main.cpp Sat Jan 19 12:13:21 2019 +0000 +++ b/main.cpp Sat Jan 19 14:55:42 2019 +0000 @@ -1,46 +1,46 @@ #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(); @@ -64,49 +64,49 @@ */ } - + void RFmove(int position,int speed){ if(2350 <= position){ position = 2350; } - RFelbow.SpeedConvert(speed); - RFelbow.SetPosition(fmap(position,850,2350,2350,850)); + //RFelbow.SpeedConvert(speed); + RFelbow.SetPosition(fmap(position,850,2350,2350,850),speed); } - + void LFmove(int position,int speed){ if(2350 <= position){ position = 2350; } - LFelbow.SpeedConvert(speed); - LFelbow.SetPosition(position); + //LFelbow.SpeedConvert(speed); + LFelbow.SetPosition(position,speed); } - + void RBmove(int position,int speed){ if(2350 <= position){ position = 2350; } - RBelbow.SpeedConvert(speed); - RBelbow.SetPosition(position); + //RBelbow.SpeedConvert(speed); + RBelbow.SetPosition(position,speed); } - + void LBmove(int position,int speed){ if(2350 <= position){ position = 2350; } - LBelbow.SpeedConvert(speed); - LBelbow.SetPosition(fmap(position,850,2350,2350,850)); + //LBelbow.SpeedConvert(speed); + LBelbow.SetPosition(fmap(position,850,2350,2350,850),speed); } - + void forwarding(){ RFmove(1600,3); LFmove(1600,3); RBmove(2200,3); LBmove(2200,3); } - + float angle = 0; int main() { //初期設定 @@ -117,12 +117,12 @@ 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); @@ -130,31 +130,60 @@ 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)); + //angle = int(20*var); + //PC.printf("%f \n",angle); + RBknee.SetPosition(downPos - 300,10); + LBknee.SetPosition(downPos - 300,10); if(Dmode == true){ - //RFmove(prePos,50); - RFknee.SetPosition(upPos); + /* + centerMove(20,0); + wait(2.0); + RBknee.SetPosition(upPos-100,10); + wait(2.0); + RBmove(behPos,25); wait(2.0); - RFmove(2350,10); + //RFmove(2350,10); + //wait(2.0); + RFknee.SetPosition(downPos-100,10); wait(2.0); - RFknee.SetPosition(downPos); + */ + + centerMove(20,0); + wait(2.0); + RFknee.SetPosition(upPos,10); wait(2.0); RFmove(prePos,25); wait(2.0); + //RFmove(2350,10); + //wait(2.0); + RFknee.SetPosition(downPos,10); + wait(2.0); + + centerMove(-20,0); + wait(2.0); + LFknee.SetPosition(upPos,10); + wait(2.0); + LFmove(prePos,25); + wait(2.0); + LFknee.SetPosition(downPos,10); + wait(2.0); + centerMove(0,0); - RBknee.SetPosition(downPos); - LBknee.SetPosition(downPos); + wait(2.0); - RBknee.SetPosition(downPos); - LBknee.SetPosition(downPos); + RBknee.SetPosition(downPos,10); + LBknee.SetPosition(downPos,10); + RBknee.SetPosition(downPos-300,10); + LBknee.SetPosition(downPos-300,10); + + //RBknee.SpeedConvert(10); + //LBknee.SpeedConvert(10); + wait(3.0); + }else{ - RFknee.SetPosition(downPos); + RFknee.SetPosition(downPos,10); forwarding(); }