NagaokaRoboticsClub_mbedTeam / Mbed OS 2021NHK_B_syudo

2021 NHK B syudo

Dependencies:   2021Bcon omni_wheel ikarashiMDC_2byte_ver Servo_softpwm NHK2021_ikarashiSV

main.cpp@2:e7910c124624, 2021-10-17 (annotated)

Committer:
nagix
Date:
Sun Oct 17 10:04:20 2021 +0000
Revision:
2:e7910c124624
Parent:
0:24bde65ddf8b
Child:
3:a564d484cd4a
miss

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nagix 0:24bde65ddf8b 1 #include "mbed.h"
nagix 0:24bde65ddf8b 2 // #include "QEI.h"
nagix 0:24bde65ddf8b 3 #include "ikarashiMDC.h"
nagix 2:e7910c124624 4 //#include "jy901.h"
nagix 0:24bde65ddf8b 5 #include "ikarashiSV.h"
nagix 0:24bde65ddf8b 6 #include "controller.h"
nagix 0:24bde65ddf8b 7 #include "pinconfig.h"
nagix 0:24bde65ddf8b 8 #include "omni_wheel.h"
nagix 0:24bde65ddf8b 9 #include "math.h"
nagix 0:24bde65ddf8b 10
nagix 0:24bde65ddf8b 11 #define PI 3.141592653589
nagix 0:24bde65ddf8b 12
nagix 0:24bde65ddf8b 13 Bcon mycon(fepTX, fepRX, fepad);
nagix 0:24bde65ddf8b 14 Serial pc(pcTX, pcRX, 115200);
nagix 0:24bde65ddf8b 15 Serial RS485(mdcTX,mdcRX,115200);
nagix 0:24bde65ddf8b 16
nagix 0:24bde65ddf8b 17 //JY901 jy(PB_9, PB_8); //sda, scl
nagix 0:24bde65ddf8b 18 DigitalOut stop(em_stop);
nagix 0:24bde65ddf8b 19
nagix 0:24bde65ddf8b 20 //QEI enc1(PA_6,PA_7,NC,100, QEI::X4_ENCODING);
nagix 0:24bde65ddf8b 21 //QEI enc2(PA_8,PA_9,NC,100, QEI::X4_ENCODING);
nagix 0:24bde65ddf8b 22 //QEI enc3(PB_6,PB_7,NC,100, QEI::X4_ENCODING);
nagix 0:24bde65ddf8b 23
nagix 0:24bde65ddf8b 24 ikarashiMDC motor[] = {
nagix 0:24bde65ddf8b 25 ikarashiMDC(0,0,SM,&RS485),/*足*/
nagix 0:24bde65ddf8b 26 ikarashiMDC(0,1,SM,&RS485),/*足*/
nagix 0:24bde65ddf8b 27 ikarashiMDC(0,2,SM,&RS485),/*足*/
nagix 0:24bde65ddf8b 28 ikarashiMDC(0,3,SM,&RS485),/*足*/
nagix 0:24bde65ddf8b 29 ikarashiMDC(1,0,SM,&RS485),/*腕昇降*/
nagix 0:24bde65ddf8b 30 ikarashiMDC(1,1,SM,&RS485),/*腕前後*/
nagix 0:24bde65ddf8b 31 };
nagix 0:24bde65ddf8b 32
nagix 0:24bde65ddf8b 33 ikarashiSV slv(PC_7,PB_10,PB_4,PB_5);
nagix 0:24bde65ddf8b 34
nagix 0:24bde65ddf8b 35 OmniWheel omni(4);
nagix 0:24bde65ddf8b 36
nagix 0:24bde65ddf8b 37 void add(){
nagix 0:24bde65ddf8b 38 slv.add_state();
nagix 0:24bde65ddf8b 39 }
nagix 0:24bde65ddf8b 40
nagix 0:24bde65ddf8b 41
nagix 0:24bde65ddf8b 42
nagix 0:24bde65ddf8b 43 int main(void){
nagix 0:24bde65ddf8b 44
nagix 0:24bde65ddf8b 45 //float x,y,z;
nagix 0:24bde65ddf8b 46 //jy.calibrateAll(50);
nagix 0:24bde65ddf8b 47
nagix 0:24bde65ddf8b 48 mycon.StartReceive();
nagix 0:24bde65ddf8b 49 uint8_t b[8];
nagix 0:24bde65ddf8b 50 int16_t stick[4];
nagix 0:24bde65ddf8b 51 double accele[4];
nagix 0:24bde65ddf8b 52 double speed[6];
nagix 0:24bde65ddf8b 53 double spin_power;
nagix 0:24bde65ddf8b 54
nagix 0:24bde65ddf8b 55
nagix 0:24bde65ddf8b 56 //int i=0;
nagix 0:24bde65ddf8b 57
nagix 0:24bde65ddf8b 58 omni.wheel[0].setRadian(PI * 1.0 / 4.0);
nagix 0:24bde65ddf8b 59 omni.wheel[1].setRadian(PI * 3.0 / 4.0);
nagix 0:24bde65ddf8b 60 omni.wheel[2].setRadian(PI * 5.0 / 4.0);
nagix 0:24bde65ddf8b 61 omni.wheel[3].setRadian(PI * 7.0 / 4.0);
nagix 0:24bde65ddf8b 62
nagix 0:24bde65ddf8b 63 while (1)
nagix 0:24bde65ddf8b 64 {
nagix 0:24bde65ddf8b 65 /*非常停止*/
nagix 2:e7910c124624 66 stop = 1;
nagix 0:24bde65ddf8b 67
nagix 0:24bde65ddf8b 68 /*ジャイロ読み取り*/
nagix 0:24bde65ddf8b 69 // x = jy.getXaxisAngle();
nagix 0:24bde65ddf8b 70 // y = jy.getYaxisAngle();
nagix 0:24bde65ddf8b 71 // z = jy.getZaxisAngle();
nagix 0:24bde65ddf8b 72 // pc.printf("x:%3.0f | y:%3.0f | z:%3.0f | ", x, y, z);
nagix 0:24bde65ddf8b 73 // x = x/180;
nagix 0:24bde65ddf8b 74 // y = y/180;
nagix 0:24bde65ddf8b 75 // z = z/180;
nagix 0:24bde65ddf8b 76
nagix 0:24bde65ddf8b 77 /*コントローラー受信*/
nagix 0:24bde65ddf8b 78 for (int i=0; i<8; i++) b[i] = mycon.getButton(i);
nagix 0:24bde65ddf8b 79 for (int i=0; i<4; i++) stick[i] = mycon.getStick(i);
nagix 0:24bde65ddf8b 80
nagix 0:24bde65ddf8b 81 for (int i=0; i<8; i++) pc.printf("%d ", b[i]);
nagix 0:24bde65ddf8b 82 pc.printf(" | ");
nagix 0:24bde65ddf8b 83 for (int i=0; i<4; i++) pc.printf("%3d ", stick[i]);
nagix 0:24bde65ddf8b 84 pc.printf(" | ");
nagix 0:24bde65ddf8b 85 if (mycon.status) pc.printf("received\r\n");
nagix 0:24bde65ddf8b 86 else pc.printf("anything error...\r\n");
nagix 0:24bde65ddf8b 87
nagix 0:24bde65ddf8b 88 for(int i=0; i<6; i++){
nagix 0:24bde65ddf8b 89 speed[i] = 0;
nagix 0:24bde65ddf8b 90 }
nagix 0:24bde65ddf8b 91
nagix 0:24bde65ddf8b 92 /*全方位制御*/
nagix 0:24bde65ddf8b 93 for(int i=0; i<4; i++){
nagix 0:24bde65ddf8b 94 if(abs(stick[i]) > 10){
nagix 0:24bde65ddf8b 95 accele[i] = stick[i]/128.0;
nagix 0:24bde65ddf8b 96 }else{
nagix 0:24bde65ddf8b 97 accele[i] = 0;
nagix 0:24bde65ddf8b 98 }
nagix 0:24bde65ddf8b 99 }
nagix 0:24bde65ddf8b 100
nagix 0:24bde65ddf8b 101 /*旋回*/
nagix 0:24bde65ddf8b 102 if(abs(stick[2]) > 10){
nagix 0:24bde65ddf8b 103 spin_power = accele[2];
nagix 0:24bde65ddf8b 104 }else{
nagix 0:24bde65ddf8b 105 spin_power = 0;
nagix 0:24bde65ddf8b 106 }
nagix 0:24bde65ddf8b 107
nagix 0:24bde65ddf8b 108 omni.computeXY(accele[0],accele[1],-spin_power);
nagix 0:24bde65ddf8b 109 for(int i = 0; i < 4; i++) speed[i] = omni.wheel[i];
nagix 0:24bde65ddf8b 110
nagix 0:24bde65ddf8b 111 /*腕昇降*/
nagix 0:24bde65ddf8b 112 if(b[0] != 0){
nagix 0:24bde65ddf8b 113 speed[4] = 0.3;
nagix 0:24bde65ddf8b 114 }else if(b[2] != 0){
nagix 0:24bde65ddf8b 115 speed[4] = -0.3;
nagix 0:24bde65ddf8b 116 }else{
nagix 0:24bde65ddf8b 117 speed[4] = 0;
nagix 0:24bde65ddf8b 118 }
nagix 0:24bde65ddf8b 119
nagix 0:24bde65ddf8b 120 /*腕前後*/
nagix 0:24bde65ddf8b 121 if(b[3] != 0){
nagix 0:24bde65ddf8b 122 speed[5] = 0.3;
nagix 0:24bde65ddf8b 123 }else if(b[1] != 0){
nagix 0:24bde65ddf8b 124 speed[5] = -0.3;
nagix 0:24bde65ddf8b 125 }else{
nagix 0:24bde65ddf8b 126 speed[5] = 0;
nagix 0:24bde65ddf8b 127 }
nagix 0:24bde65ddf8b 128
nagix 0:24bde65ddf8b 129 for(int i=0; i<6; i++) motor[i].setSpeed(speed[i]);
nagix 0:24bde65ddf8b 130
nagix 0:24bde65ddf8b 131 /*
nagix 0:24bde65ddf8b 132 if(b[0]==1){
nagix 0:24bde65ddf8b 133 motor[0].setSpeed(0.3);
nagix 0:24bde65ddf8b 134 }else if(b[1]){
nagix 0:24bde65ddf8b 135 motor[1].setSpeed(0.3);
nagix 0:24bde65ddf8b 136 }else if(b[2]){
nagix 0:24bde65ddf8b 137 motor[2].setSpeed(0.3);
nagix 0:24bde65ddf8b 138 }else if(b[3]){
nagix 0:24bde65ddf8b 139 motor[3].setSpeed(0.3);
nagix 0:24bde65ddf8b 140 }else if(b[4]){
nagix 0:24bde65ddf8b 141 motor[0].setSpeed(-0.3);
nagix 0:24bde65ddf8b 142 }else if(b[5]){
nagix 0:24bde65ddf8b 143 motor[1].setSpeed(-0.3);
nagix 0:24bde65ddf8b 144 }else if(b[6]){
nagix 0:24bde65ddf8b 145 motor[2].setSpeed(-0.3);
nagix 0:24bde65ddf8b 146 }else if(b[7]){
nagix 0:24bde65ddf8b 147 motor[3].setSpeed(-0.3);
nagix 0:24bde65ddf8b 148 }else{
nagix 0:24bde65ddf8b 149 motor[0].setSpeed(0);
nagix 0:24bde65ddf8b 150 motor[1].setSpeed(0);
nagix 0:24bde65ddf8b 151 motor[2].setSpeed(0);
nagix 0:24bde65ddf8b 152 motor[3].setSpeed(0);
nagix 0:24bde65ddf8b 153 }*/
nagix 0:24bde65ddf8b 154 }
nagix 0:24bde65ddf8b 155
nagix 0:24bde65ddf8b 156 }