File content as of revision 0:158455b60200:

#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);
   }
}