File content as of revision 1:b2edadf8c3d9:

#include "mbed.h"
#include "R1370P.h"

Serial pc(USBTX, USBRX);
R1370P gyro(PA_9,PA_10);

SPISlave spi(PA_7,PA_6,PA_5,PA_4);

PwmOut servo1(PA_3);  //servo1 = アサリつかみ（手前）
PwmOut servo2(PB_5);  //servo2 = アサリつかみ（奥）
PwmOut servo3(PA_11);  //servo3 = アサリアーム
PwmOut servo4(PA_8);  //servo4 = ガチアサリアーム

DigitalOut out(PA_1);  //エアシリンダー

int main()
{

    int flag1=0, flag2=0, flag3=0, flag4=0, flag5=0;
    int data1;
    int old_data;

    servo1.period_ms(20);
    servo2.period_ms(20);
    servo3.period_ms(20);
    servo4.period_ms(20);

    spi.format(16,3);
    spi.frequency(1000000);

    //int debug=0;

    double angle, reply_angle;
    int a;
    gyro.initialize();


    while(1) {

        //angle = gyro.getAngle();
        //printf("%f\r\n",angle);
        if(spi.receive()) {
            data1=spi.read();

            angle = gyro.getAngle();
            if(angle >= 0) {  //反時計回りに0~359°となるよう修正
               a = angle / 360;
               reply_angle = (angle - (360 * a))*100;  //現在の角度
            } else {
               a = angle / 360;
               reply_angle = (360 + angle - (360 * a))*100;
            }
            
            //printf("angle = %f\r\n",reply_angle);
            spi.reply(reply_angle);

            /*printf("%d\r\n",debug);
            debug++;
            if(debug>100) debug=0;*/
        }

        if(data1 != old_data) {
            if((flag1 == 1) && (data1 == 1)) {
                servo1.pulsewidth_us(1000);
                flag1=0;
            } else if((flag1 == 0) && (data1 == 1)) {
                servo1.pulsewidth_us(1800);
                flag1=1;
            }

            if((flag2 == 1) && (data1 == 2)) {
                servo2.pulsewidth_us(1200);
                flag2=0;
            } else if((flag2 == 0) && (data1 == 2)) {
                servo2.pulsewidth_us(1800);
                flag2=1;
            }

            if((flag3 == 1) && (data1 == 3)) {
                servo3.pulsewidth_us(800);
                flag3=0;
            } else if((flag3 == 0) && (data1 == 3)) {
                servo3.pulsewidth_us(1250);
                flag3=1;
            }

            if((flag4 == 1) && (data1 == 4)) {
                servo4.pulsewidth_us(1500);
                flag4=0;
            } else if((flag4 == 0) && (data1 == 4)) {
                servo4.pulsewidth_us(2000);
                flag4=1;
            }

            if((flag5 == 1) && (data1 == 5)) {
                out = 1;
                flag5=0;
            } else if((flag5 == 0) && (data1 == 5)) {
                out = 0;
                flag5=1;
            }
        }

        old_data = data1;

    }
}