NagaokaRoboticsClub_mbedTeam / Mbed OS hayatoShooter

2017ロボコンはやとブーメランプログラム

Dependencies:   PID QEI ikarashiMDC recieveController omni

main.cpp@20:d052a0679309, 2017-11-06 (annotated)

Committer:
WAT34
Date:
Mon Nov 06 18:54:45 2017 +0900
Revision:
20:d052a0679309
Parent:
18:4b629221c215 
belt pided

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WAT34 1:35e874e10602 1 #include "mbed.h"
WAT34 1:35e874e10602 2 #include "hayatoBoomerang.h"
WAT34 5:9c52e6c14c87 3 #include "recieveController.h"
WAT34 6:0bee4b2bb400 4 #include "omni.h"
WAT34 13:d01356a29b93 5 #include "gakugaku.h"
WAT34 6:0bee4b2bb400 6
WAT34 1:35e874e10602 7
WAT34 4:c2b09fa31492 8 Serial rs485(PC_10,PC_11,38400);
WAT34 5:9c52e6c14c87 9 Serial serial(USBTX,USBRX);
WAT34 17:311aed3cad15 10 Serial esp(PA_9,PA_10,115200);
WAT34 17:311aed3cad15 11 Serial spiral(PA_0,PA_1,38400);
WAT34 4:c2b09fa31492 12 PwmOut led(LED1);
WAT34 4:c2b09fa31492 13 DigitalIn button(BUTTON1);
WAT34 17:311aed3cad15 14 boomerang boom(&rs485,&spiral);
WAT34 17:311aed3cad15 15 gakugaku fishingPole(&spiral);
WAT34 17:311aed3cad15 16 Ticker debugesp,ticker;
eil4nyqn 8:c47cf4e0230c 17 recieveController con(PC_12,PD_2,198);
WAT34 18:4b629221c215 18 BusIn modeChange(PA_4,PB_0,PC_2,PC_1);
WAT34 18:4b629221c215 19 BusOut modeStatus(NC,NC,NC,NC);
WAT34 18:4b629221c215 20 BusOut angleState(NC,NC,NC);
WAT34 4:c2b09fa31492 21
WAT34 17:311aed3cad15 22 double angle = 10;
WAT34 17:311aed3cad15 23
WAT34 18:4b629221c215 24
WAT34 17:311aed3cad15 25 void update()
WAT34 17:311aed3cad15 26 {
WAT34 17:311aed3cad15 27 boom.update();
WAT34 17:311aed3cad15 28 fishingPole.update();
WAT34 17:311aed3cad15 29 }
WAT34 4:c2b09fa31492 30 void init()
WAT34 4:c2b09fa31492 31 {
WAT34 4:c2b09fa31492 32 button.mode(PullUp);
WAT34 4:c2b09fa31492 33
WAT34 5:9c52e6c14c87 34 serial.baud(115200);
WAT34 4:c2b09fa31492 35 //boom.calibrateArm();
WAT34 4:c2b09fa31492 36 boom.setTargetMotorSpeed(0);
WAT34 4:c2b09fa31492 37 }
WAT34 1:35e874e10602 38
WAT34 17:311aed3cad15 39 void debugout()
WAT34 17:311aed3cad15 40 {
WAT34 17:311aed3cad15 41 esp.printf("angle:%f\n",angle);
WAT34 17:311aed3cad15 42 }
WAT34 17:311aed3cad15 43
WAT34 1:35e874e10602 44 int main() {
WAT34 4:c2b09fa31492 45 init();
WAT34 18:4b629221c215 46 double turnRate = 0;
WAT34 18:4b629221c215 47 double flywheelspeed;
WAT34 13:d01356a29b93 48 double angling;
WAT34 13:d01356a29b93 49 bool prevAnglechange = false,prevSwing=false;
WAT34 20:d052a0679309 50 bool swinging = false;
WAT34 18:4b629221c215 51 char data[20],*data2,*data3;
WAT34 18:4b629221c215 52 modeChange.mode(PullUp);
WAT34 18:4b629221c215 53 if((15-modeChange.read())%3 == 0)
WAT34 18:4b629221c215 54 flywheelspeed =13000;
WAT34 18:4b629221c215 55 else if(((15-modeChange.read())%3) == 1)
WAT34 18:4b629221c215 56 flywheelspeed =13000*3.0/4.0;
WAT34 18:4b629221c215 57 else if(((15-modeChange.read())%3) == 2)
WAT34 18:4b629221c215 58 flywheelspeed = 13000*2.0/3.0;
WAT34 17:311aed3cad15 59 //debugesp.attach(&debugout,3);
WAT34 5:9c52e6c14c87 60 // commThread.start(&comm);
WAT34 4:c2b09fa31492 61 while(true)
WAT34 4:c2b09fa31492 62 {
WAT34 18:4b629221c215 63 modeStatus = 15-modeChange.read();
WAT34 18:4b629221c215 64 if((con.buttons[0] >>9)%2 )
WAT34 17:311aed3cad15 65 {
WAT34 6:0bee4b2bb400 66 boom.fire();
WAT34 17:311aed3cad15 67 }
WAT34 6:0bee4b2bb400 68
WAT34 18:4b629221c215 69 if((con.buttons[0] >>10)%2)
WAT34 18:4b629221c215 70 boom.setTargetMotorSpeed(flywheelspeed);
WAT34 18:4b629221c215 71
WAT34 6:0bee4b2bb400 72 else
WAT34 6:0bee4b2bb400 73 boom.setTargetMotorSpeed(0);
WAT34 6:0bee4b2bb400 74
WAT34 16:5f205692b14d 75
WAT34 18:4b629221c215 76 if((con.buttons[0]>>12)%2) //square
WAT34 16:5f205692b14d 77 angle = 80;
WAT34 18:4b629221c215 78 if((con.buttons[0]>>14)%2) //cross
WAT34 16:5f205692b14d 79 angle = 45;
WAT34 18:4b629221c215 80 if((con.buttons[0]>>13)%2) //circle
WAT34 16:5f205692b14d 81 angle = 25;
WAT34 18:4b629221c215 82 if((con.buttons[0]>>11)%2) //triangle
WAT34 13:d01356a29b93 83 angle = 10;
WAT34 18:4b629221c215 84 if((con.buttons[0]>>2)%2 )
WAT34 13:d01356a29b93 85 {
WAT34 13:d01356a29b93 86 if(!prevSwing)
WAT34 20:d052a0679309 87 swinging != swinging;
WAT34 13:d01356a29b93 88 prevSwing = true;
WAT34 13:d01356a29b93 89 }else{
WAT34 13:d01356a29b93 90 prevSwing = false;
WAT34 13:d01356a29b93 91 }
WAT34 20:d052a0679309 92 fishingPole.Swing(swinging);
WAT34 18:4b629221c215 93 if(con.buttons[4] < 117 )
WAT34 7:baf16d0c14e7 94 {
WAT34 7:baf16d0c14e7 95 if(!prevAnglechange)
WAT34 17:311aed3cad15 96 angle += 2.5;
WAT34 7:baf16d0c14e7 97 prevAnglechange =true;
WAT34 12:9d21189a8eb0 98 }else
WAT34 18:4b629221c215 99 if(con.buttons[4] > 137)
WAT34 7:baf16d0c14e7 100 {
WAT34 7:baf16d0c14e7 101 if(!prevAnglechange)
WAT34 17:311aed3cad15 102 angle -= 2.5;
WAT34 7:baf16d0c14e7 103 prevAnglechange =true;
WAT34 7:baf16d0c14e7 104 }else{
WAT34 7:baf16d0c14e7 105 prevAnglechange =false;
WAT34 7:baf16d0c14e7 106 }
WAT34 6:0bee4b2bb400 107 if(angle < 5){
WAT34 6:0bee4b2bb400 108 angle = 5;
WAT34 6:0bee4b2bb400 109 }
WAT34 17:311aed3cad15 110 if(angle > 90 ){
WAT34 17:311aed3cad15 111 angle = 90;
WAT34 6:0bee4b2bb400 112 }
WAT34 6:0bee4b2bb400 113
WAT34 18:4b629221c215 114 if((con.buttons[3] >50))
WAT34 18:4b629221c215 115 turnRate = con.buttons[3]/500.0;
WAT34 18:4b629221c215 116 else if((con.buttons[6]/4080 > 50))
WAT34 18:4b629221c215 117 turnRate = -(con.buttons[6]/4080)/500.0;
WAT34 12:9d21189a8eb0 118 else
WAT34 18:4b629221c215 119 turnRate = 0;
WAT34 12:9d21189a8eb0 120
WAT34 18:4b629221c215 121 if((con.buttons[0]%2))
WAT34 17:311aed3cad15 122 angling = -0.4;
WAT34 18:4b629221c215 123 else if((con.buttons[0]>>1)%2)
WAT34 18:4b629221c215 124 angling = 0.2;
WAT34 13:d01356a29b93 125 else
WAT34 13:d01356a29b93 126 angling = 0;
WAT34 13:d01356a29b93 127 fishingPole.setAnglingSpeed(angling);
WAT34 13:d01356a29b93 128
WAT34 18:4b629221c215 129 if((con.buttons[0]>>8)%2)
WAT34 13:d01356a29b93 130 fishingPole.Deploy();
WAT34 13:d01356a29b93 131
WAT34 17:311aed3cad15 132 update();
WAT34 12:9d21189a8eb0 133
WAT34 6:0bee4b2bb400 134 boom.setTargetArmAngle(angle);
WAT34 18:4b629221c215 135 angleState = int(angle/90.0*7);
WAT34 6:0bee4b2bb400 136 led = boom.beltSpeed;
WAT34 12:9d21189a8eb0 137 boom.move(con.buttons[1],con.buttons[2],turnRate);
WAT34 20:d052a0679309 138 //serial.printf("controller:%d\n\r",con.buttons[0]);
WAT34 5:9c52e6c14c87 139 con.update();
WAT34 4:c2b09fa31492 140
WAT34 4:c2b09fa31492 141 }
WAT34 4:c2b09fa31492 142
WAT34 1:35e874e10602 143 }