NagaokaRoboticsClub_mbedTeam / Mbed OS hayatoShooter

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

Dependencies:   PID QEI ikarashiMDC recieveController omni

main.cpp@25:fd4fb86b4148, 2017-12-01 (annotated)

Committer:
WAT34
Date:
Fri Dec 01 18:16:33 2017 +0900
Revision:
25:fd4fb86b4148
Parent:
24:050d78bb5b69 
hopee it workks

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 18:4b629221c215 50 char data[20],*data2,*data3;
WAT34 18:4b629221c215 51 modeChange.mode(PullUp);
WAT34 18:4b629221c215 52 if((15-modeChange.read())%3 == 0)
WAT34 18:4b629221c215 53 flywheelspeed =13000;
WAT34 18:4b629221c215 54 else if(((15-modeChange.read())%3) == 1)
WAT34 18:4b629221c215 55 flywheelspeed =13000*3.0/4.0;
WAT34 18:4b629221c215 56 else if(((15-modeChange.read())%3) == 2)
WAT34 18:4b629221c215 57 flywheelspeed = 13000*2.0/3.0;
WAT34 17:311aed3cad15 58 //debugesp.attach(&debugout,3);
WAT34 5:9c52e6c14c87 59 // commThread.start(&comm);
WAT34 4:c2b09fa31492 60 while(true)
WAT34 4:c2b09fa31492 61 {
WAT34 18:4b629221c215 62 modeStatus = 15-modeChange.read();
WAT34 24:050d78bb5b69 63 if((con.buttons[0] >>7)%2 )
WAT34 17:311aed3cad15 64 {
WAT34 6:0bee4b2bb400 65 boom.fire();
WAT34 17:311aed3cad15 66 }
WAT34 6:0bee4b2bb400 67
WAT34 24:050d78bb5b69 68 if((con.buttons[0] >>8)%2)
WAT34 18:4b629221c215 69 boom.setTargetMotorSpeed(flywheelspeed);
WAT34 18:4b629221c215 70
WAT34 6:0bee4b2bb400 71 else
WAT34 6:0bee4b2bb400 72 boom.setTargetMotorSpeed(0);
WAT34 6:0bee4b2bb400 73
WAT34 16:5f205692b14d 74
WAT34 24:050d78bb5b69 75 if((con.buttons[0]>>3)%2) //square
WAT34 16:5f205692b14d 76 angle = 80;
WAT34 24:050d78bb5b69 77 if((con.buttons[0]>>4)%2) //cross
WAT34 25:fd4fb86b4148 78 angle = 37.5;
WAT34 24:050d78bb5b69 79 if((con.buttons[0]>>5)%2) //circle
WAT34 16:5f205692b14d 80 angle = 25;
WAT34 24:050d78bb5b69 81 if((con.buttons[0]>>6)%2) //triangle
WAT34 13:d01356a29b93 82 angle = 10;
WAT34 24:050d78bb5b69 83 if((con.buttons[0]>>13)%2 )
WAT34 13:d01356a29b93 84 {
WAT34 13:d01356a29b93 85 if(!prevSwing)
WAT34 13:d01356a29b93 86 fishingPole.toggleSwing();
WAT34 13:d01356a29b93 87 prevSwing = true;
WAT34 13:d01356a29b93 88 }else{
WAT34 13:d01356a29b93 89 prevSwing = false;
WAT34 13:d01356a29b93 90 }
WAT34 24:050d78bb5b69 91 if(con.buttons[3] < 117 )
WAT34 7:baf16d0c14e7 92 {
WAT34 7:baf16d0c14e7 93 if(!prevAnglechange)
WAT34 17:311aed3cad15 94 angle += 2.5;
WAT34 7:baf16d0c14e7 95 prevAnglechange =true;
WAT34 12:9d21189a8eb0 96 }else
WAT34 24:050d78bb5b69 97 if(con.buttons[3] > 137)
WAT34 7:baf16d0c14e7 98 {
WAT34 7:baf16d0c14e7 99 if(!prevAnglechange)
WAT34 17:311aed3cad15 100 angle -= 2.5;
WAT34 7:baf16d0c14e7 101 prevAnglechange =true;
WAT34 7:baf16d0c14e7 102 }else{
WAT34 7:baf16d0c14e7 103 prevAnglechange =false;
WAT34 7:baf16d0c14e7 104 }
WAT34 6:0bee4b2bb400 105 if(angle < 5){
WAT34 6:0bee4b2bb400 106 angle = 5;
WAT34 6:0bee4b2bb400 107 }
WAT34 17:311aed3cad15 108 if(angle > 90 ){
WAT34 17:311aed3cad15 109 angle = 90;
WAT34 6:0bee4b2bb400 110 }
WAT34 6:0bee4b2bb400 111
WAT34 24:050d78bb5b69 112 if((con.buttons[0] >> 9)%2)
WAT34 24:050d78bb5b69 113 turnRate =-.2; //turn at .2 power
WAT34 24:050d78bb5b69 114 else if((con.buttons[0] >> 10)%2)
WAT34 24:050d78bb5b69 115 turnRate = .2;
WAT34 12:9d21189a8eb0 116 else
WAT34 18:4b629221c215 117 turnRate = 0;
WAT34 12:9d21189a8eb0 118
WAT34 25:fd4fb86b4148 119 if((con.buttons[0]>>12)%2)
WAT34 25:fd4fb86b4148 120 angling = -0.2;
WAT34 25:fd4fb86b4148 121 else if((con.buttons[0]>>14)%2)
WAT34 25:fd4fb86b4148 122 angling = 0.4;
WAT34 13:d01356a29b93 123 else
WAT34 13:d01356a29b93 124 angling = 0;
WAT34 13:d01356a29b93 125 fishingPole.setAnglingSpeed(angling);
WAT34 13:d01356a29b93 126
WAT34 24:050d78bb5b69 127 if((con.buttons[0]>>15)%2)
WAT34 13:d01356a29b93 128 fishingPole.Deploy();
WAT34 13:d01356a29b93 129
WAT34 17:311aed3cad15 130 update();
WAT34 12:9d21189a8eb0 131
WAT34 6:0bee4b2bb400 132 boom.setTargetArmAngle(angle);
WAT34 18:4b629221c215 133 angleState = int(angle/90.0*7);
WAT34 6:0bee4b2bb400 134 led = boom.beltSpeed;
WAT34 12:9d21189a8eb0 135 boom.move(con.buttons[1],con.buttons[2],turnRate);
WAT34 18:4b629221c215 136 serial.printf("controller:%d\n\r",con.buttons[0]);
WAT34 5:9c52e6c14c87 137 con.update();
WAT34 4:c2b09fa31492 138
WAT34 4:c2b09fa31492 139 }
WAT34 4:c2b09fa31492 140
WAT34 1:35e874e10602 141 }