ケンタ ミヤザキ
/
nhk_auto_06
kkkk
main.cpp
- Committer:
- kenken0721
- Date:
- 2019-01-19
- Revision:
- 1:681efc14eba9
- Parent:
- 0:158455b60200
File content as of revision 1:681efc14eba9:
#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),speed); } void LFmove(int position,int speed){ if(2350 <= position){ position = 2350; } //LFelbow.SpeedConvert(speed); LFelbow.SetPosition(position,speed); } void RBmove(int position,int speed){ if(2350 <= position){ position = 2350; } //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),speed); } 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); RBknee.SetPosition(downPos - 300,10); LBknee.SetPosition(downPos - 300,10); if(Dmode == true){ /* centerMove(20,0); wait(2.0); RBknee.SetPosition(upPos-100,10); wait(2.0); RBmove(behPos,25); wait(2.0); //RFmove(2350,10); //wait(2.0); RFknee.SetPosition(downPos-100,10); wait(2.0); */ 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); wait(2.0); 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,10); forwarding(); } wait(0.1); } }