手動機アーム、ヌクレオ基盤のプログラムです(9/4)

Dependencies:   RcServo mbed

Fork of a_SPI_slave_arm_shudouki by F^3 RC 2班

main.cpp

Committer:
yoka06
Date:
2017-09-04
Revision:
4:ea846eabda0e
Parent:
3:f200ae8fea88

File content as of revision 4:ea846eabda0e:

#include "mbed.h"
#include "rc-servo.h"
#include "motor_1.h"
#include "motor_2.h"

int a,b,c,d,e,f,g,h;
//Serial pc(USBTX,USBRX);
SPISlave spi(PA_7,PA_6,PA_5,PA_4);

RcServo sA(PB_0,500,2400,180);       //PwmOutのpin,minpulsewidth,maxpulsewidth,motionrange
RcServo sB(PA_9,500,2400,180);
RcServo sC(PA_10,500,2100,180);
RcServo sD(PA_11,500,3000,180);
RcServo sE(PA_2,500,2400,180);
RcServo sF(PA_3,500,3000,180);
RcServo sG(PA_8,500,2400,180);
M2 mH(PB_6,PA_0,PA_1);     //pwm*1,digital*2

int main()
{
    spi.format(8,3);
    spi.frequency(1000000);


    while(1) {
        if(spi.receive()) {

            a=spi.read() & 0b1;
            b=(spi.read() & 0b10)>>1;
            c=(spi.read() & 0b100)>>2;
            d=(spi.read() & 0b1000)>>3;
            e=(spi.read() & 0b10000)>>4;
            f=(spi.read() & 0b100000)>>5;
            g=(spi.read() & 0b1000000)>>6;
            h=(spi.read() & 0b10000000)>>7;
            //pc.printf("a:%d\t b:%d\t c:%d\t d:%d\t e:%d\t f:%d\t g:%d\t h:%d\t send:%d\r\n",a,b,c,d,e,f,g,h,spi.read());
        }


        if(a==1) {
            //if((ButtonState >> BUTTONUP)&1 == 1) { // ▲で燃料用アーム展開
            sD.set_deg(80);
            wait(0.7);
            sA.set_deg(118);
            wait(1.2);
            sB.set_deg(0);
            sC.set_deg(180);

        }



        if(b==1) {
            //if((ButtonState >> BUTTONTRIANGEL)&1 == 1) { // △で燃料回収
            sB.set_deg(180);
            sC.set_deg(0);
            wait(0.7);
            sA.set_deg(28);
            wait(0.7);
            sD.set_deg(0);
            sE.set_deg(120);



        }

        if(c==1) {
            //if((ButtonState >> BUTTONLEFT)&1 == 1) { // ◀で燃料掃き出し
            mH.move(1);
            wait(1.2);
            mH.move(0);

        }

        if(d==1) {
            //if((ButtonState >> BUTTONCIRCLE)&1 == 1) { // 〇でタンク回収
            sA.set_deg(118);
            sE.set_deg(30);
            wait(0.7);
            sD.set_deg(180);
            wait(0.7);
            sE.set_deg(120);
            wait(0.7);
            sD.set_deg(80);
            wait(0.7);
            sE.set_deg(90);
            sA.set_deg(27);



        }

        if(e==1) {
            //if((ButtonState >> BUTTONRIGHT)&1 == 1) { // ‣でタンク掃き出し
            mH.move(-1);
            wait(1.2);
            mH.move(0);
        }

        if(f==1) {
            //if((ButtonState >> BUTTONDOWN)&1 == 1) { // ▼で待機

            sF.set_deg(0);
            sG.set_deg(100);
            sE.set_deg(5);
            sD.set_deg(180);
        }

        if(g==1) {
            //if((ButtonState >> BUTTONCROSS)&1 == 1) { // ×でサンプル受け取り
            sG.set_deg(30);
            wait(0.7);
            sF.set_deg(87);

        }

        if(h==1) {
            //if((ButtonState >> BUTTONSQUARE)&1 == 1) { // □でサンプル設置
            sG.set_deg(120);
        }

    }

}