NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2017_octopus2_arms

タコ　腕

Dependencies:   2017NHKpin_config FEP ikarashiMDC

Fork of NHK2017_octopus2 by NagaokaRoboticsClub_mbedTeam

main.cpp@12:486068800862, 2017-09-11 (annotated)

Committer:
uchitake
Date:
Mon Sep 11 20:31:59 2017 +0900
Revision:
12:486068800862
Parent:
6:fe9767a50891 
nowork

Who changed what in which revision?

UserRevisionLine numberNew contents of line
uchitake 1:845af5425eec 1 #include "mbed.h"
uchitake 1:845af5425eec 2 #include "pin_config.h"
uchitake 12:486068800862 3 #include "wheel_unit.h"
uchitake 12:486068800862 4 #include "slider.h"
uchitake 12:486068800862 5 #include "PID_controller.h"
uchitake 12:486068800862 6 #include "controller.h"
uchitake 12:486068800862 7
uchitake 12:486068800862 8 DigitalOut powerSwitch(MDstop);
uchitake 12:486068800862 9 DigitalOut RS485Controller(PWM1);
uchitake 12:486068800862 10 Serial RS485(MDTX, MDRX, RS485_BAUD);
uchitake 12:486068800862 11
uchitake 12:486068800862 12 WheelUnit quadOmni(&RS485Controller, &RS485);
uchitake 12:486068800862 13 Slider slider(&RS485Controller, &RS485);
uchitake 12:486068800862 14 PIDC pid(HMCsda, HMCscl, KC, TI, TD, INTERVAL);
uchitake 12:486068800862 15 Controller pad(XBee1TX, XBee1RX, ADDR);
uchitake 12:486068800862 16 ikarashiMDC fukuda(&RS485Controller, 1, 1, SM, &RS485);
uchitake 12:486068800862 17 ikarashiMDC crab(&RS485Controller, 1, 2, SM, &RS485);
uchitake 12:486068800862 18 ikarashiMDC joint(&RS485Controller, 1, 3, SM, &RS485);
uchitake 12:486068800862 19
uchitake 12:486068800862 20 volatile bool receiveSuccessed;
uchitake 12:486068800862 21
uchitake 12:486068800862 22 Serial pc(USBTX, USBRX, 115200);
uchitake 12:486068800862 23 DigitalOut debugLED[3] = {
uchitake 12:486068800862 24 DigitalOut(DebugLED3),
uchitake 12:486068800862 25 DigitalOut(DebugLED4),
uchitake 12:486068800862 26 DigitalOut(DebugLED5)
uchitake 12:486068800862 27 };
uchitake 12:486068800862 28
uchitake 12:486068800862 29 void getpad()
uchitake 12:486068800862 30 {
uchitake 12:486068800862 31 while(true) {
uchitake 12:486068800862 32 receiveSuccessed = pad.receiveState();
uchitake 12:486068800862 33 pc.printf("%d\n\r", receiveSuccessed);
uchitake 12:486068800862 34 wait(0.0);
uchitake 12:486068800862 35 debugLED[0] = receiveSuccessed;
uchitake 12:486068800862 36 }
uchitake 12:486068800862 37 }
uchitake 12:486068800862 38
uchitake 12:486068800862 39 void getPID()
uchitake 12:486068800862 40 {
uchitake 12:486068800862 41 debugLED[1] = receiveSuccessed;
uchitake 12:486068800862 42 if(receiveSuccessed) {
uchitake 12:486068800862 43 pid.confirm();
uchitake 12:486068800862 44 }
uchitake 12:486068800862 45 }
uchitake 12:486068800862 46
uchitake 12:486068800862 47 void allStop()
uchitake 12:486068800862 48 {
uchitake 12:486068800862 49 quadOmni.moveXY(0, 0, 0);
uchitake 12:486068800862 50 slider.slide(0);
uchitake 12:486068800862 51 fukuda.setSpeed(0);
uchitake 12:486068800862 52 crab.setSpeed(0);
uchitake 12:486068800862 53 joint.setSpeed(0);
uchitake 12:486068800862 54 }
uchitake 1:845af5425eec 55
uchitake 12:486068800862 56 void moveWheels()
uchitake 12:486068800862 57 {
uchitake 12:486068800862 58 if(pad.getNorm(1) > 0.5) {
uchitake 12:486068800862 59 pid.PID::setSetPoint((pad.getRadian(1) - PI / 2) * (180.0 / PI));
uchitake 12:486068800862 60 }
uchitake 12:486068800862 61 quadOmni.moveXY(
uchitake 12:486068800862 62 pad.getStick(0) / 2.0,
uchitake 12:486068800862 63 -pad.getStick(1) /2.0,
uchitake 12:486068800862 64 pid.getCalculationResult()
uchitake 12:486068800862 65 );
uchitake 12:486068800862 66 }
uchitake 12:486068800862 67
uchitake 12:486068800862 68 void moveMach()
uchitake 12:486068800862 69 {
uchitake 12:486068800862 70 // if(!pad.getButton1(0)) motor.moveSlider(ARM_MAX_SPEED);
uchitake 12:486068800862 71 // if(!pad.getButton1(1)) motor.moveSlider(-ARM_MAX_SPEED);
uchitake 12:486068800862 72 // if(pad.getButton1(0) && pad.getButton1(1)) motor.moveSlider(0);
uchitake 12:486068800862 73 //
uchitake 12:486068800862 74 // if(!pad.getButton1(3)) motor.shakeHead(ARM_MAX_SPEED);
uchitake 12:486068800862 75 // if(!pad.getButton1(4)) motor.shakeHead(-ARM_MAX_SPEED);
uchitake 12:486068800862 76 // if(pad.getButton1(3) && pad.getButton1(4)) motor.shakeHead(0);
uchitake 12:486068800862 77 //
uchitake 12:486068800862 78 if(!pad.getButton1(5)) fukuda.setSpeed(0.6);
uchitake 12:486068800862 79 if(!pad.getButton1(6)) fukuda.setSpeed(-0.6);
uchitake 12:486068800862 80 if(pad.getButton1(5) && pad.getButton1(6)) fukuda.setSpeed(-0.0);
uchitake 12:486068800862 81
uchitake 12:486068800862 82 //
uchitake 12:486068800862 83 // if(!pad.getButton1(2)) {
uchitake 12:486068800862 84 // motor.destroy(DESTROY_MAX_SPEED);
uchitake 12:486068800862 85 // } else {
uchitake 12:486068800862 86 // motor.destroy(0);
uchitake 12:486068800862 87 // }
uchitake 12:486068800862 88 //
uchitake 12:486068800862 89 if(!pad.getButton2(1)) slider.release();
uchitake 12:486068800862 90 if(!pad.getButton2(3)) powerSwitch = 0;
uchitake 12:486068800862 91 }
uchitake 12:486068800862 92
uchitake 12:486068800862 93 void moveMotors()
uchitake 12:486068800862 94 {
uchitake 12:486068800862 95 if(receiveSuccessed) {
uchitake 12:486068800862 96 moveWheels();
uchitake 12:486068800862 97 moveMach();
uchitake 12:486068800862 98 debugLED[2] = receiveSuccessed;
uchitake 12:486068800862 99 } else {
uchitake 12:486068800862 100 allStop();
uchitake 12:486068800862 101 }
uchitake 12:486068800862 102 }
uchitake 12:486068800862 103
uchitake 12:486068800862 104 void LChika()
uchitake 12:486068800862 105 {
uchitake 12:486068800862 106 for(int i = 0; i < 3; i++) {
uchitake 12:486068800862 107 debugLED[i] = true;
uchitake 12:486068800862 108 wait(0.1);
uchitake 12:486068800862 109 debugLED[i] = false;
uchitake 12:486068800862 110 }
uchitake 12:486068800862 111 }
uchitake 1:845af5425eec 112
uchitake 1:845af5425eec 113 int main()
uchitake 1:845af5425eec 114 {
uchitake 12:486068800862 115 LChika();
uchitake 12:486068800862 116 fukuda.braking = true;
uchitake 12:486068800862 117 crab.braking = true;
uchitake 12:486068800862 118 joint.braking = true;
uchitake 12:486068800862 119 receiveSuccessed = false;
uchitake 12:486068800862 120 powerSwitch = true;
uchitake 12:486068800862 121 LChika();
uchitake 12:486068800862 122
uchitake 12:486068800862 123 int error = 0;
uchitake 12:486068800862 124 while(true) {
uchitake 1:845af5425eec 125 wait(INTERVAL);
uchitake 12:486068800862 126 getpad();
uchitake 12:486068800862 127 getPID();
uchitake 12:486068800862 128 moveMotors();
uchitake 12:486068800862 129
uchitake 12:486068800862 130 error++;
uchitake 12:486068800862 131 if(receiveSuccessed) error = 0;
uchitake 12:486068800862 132 if(error > 10) powerSwitch = 0;
uchitake 1:845af5425eec 133 }
uchitake 1:845af5425eec 134 }