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