NagaokaRoboticsClub_mbedTeam / Mbed OS hayatoShooter

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

Dependencies:   PID QEI ikarashiMDC recieveController omni

main.cpp@16:5f205692b14d, 2017-10-06 (annotated)

Committer:
WAT34
Date:
Fri Oct 06 21:59:16 2017 +0900
Revision:
16:5f205692b14d
Parent:
14:5cc44bec9cfc
Child:
17:311aed3cad15
need to fix bug

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 4:c2b09fa31492 10 PwmOut led(LED1);
WAT34 4:c2b09fa31492 11 DigitalIn button(BUTTON1);
WAT34 4:c2b09fa31492 12 boomerang boom(&rs485);
WAT34 13:d01356a29b93 13 gakugaku fishingPole(&rs485);
eil4nyqn 8:c47cf4e0230c 14 recieveController con(PC_12,PD_2,198);
WAT34 4:c2b09fa31492 15
WAT34 4:c2b09fa31492 16 void init()
WAT34 4:c2b09fa31492 17 {
WAT34 4:c2b09fa31492 18 button.mode(PullUp);
WAT34 4:c2b09fa31492 19
WAT34 5:9c52e6c14c87 20 serial.baud(115200);
WAT34 4:c2b09fa31492 21 //boom.calibrateArm();
WAT34 4:c2b09fa31492 22 boom.setTargetMotorSpeed(0);
WAT34 4:c2b09fa31492 23 }
WAT34 1:35e874e10602 24
WAT34 1:35e874e10602 25 int main() {
WAT34 4:c2b09fa31492 26 init();
WAT34 13:d01356a29b93 27 int angle = 10,turnRate = 127;
WAT34 13:d01356a29b93 28 double angling;
WAT34 13:d01356a29b93 29 bool prevAnglechange = false,prevSwing=false;
WAT34 5:9c52e6c14c87 30 char data[20],*data2,*data3;
WAT34 5:9c52e6c14c87 31 // commThread.start(&comm);
WAT34 6:0bee4b2bb400 32 // ticker.attach(&updater,0.01);
WAT34 4:c2b09fa31492 33 while(true)
WAT34 4:c2b09fa31492 34 {
WAT34 13:d01356a29b93 35 if((con.buttons[0] >>7)%2 )
WAT34 6:0bee4b2bb400 36 boom.fire();
WAT34 6:0bee4b2bb400 37
WAT34 13:d01356a29b93 38 if((con.buttons[0] >>8)%2)
WAT34 13:d01356a29b93 39 boom.setTargetMotorSpeed(13000);
WAT34 6:0bee4b2bb400 40 else
WAT34 6:0bee4b2bb400 41 boom.setTargetMotorSpeed(0);
WAT34 6:0bee4b2bb400 42
WAT34 16:5f205692b14d 43
WAT34 16:5f205692b14d 44 if((con.buttons[0]>>3)%2) //square
WAT34 16:5f205692b14d 45 angle = 80;
WAT34 16:5f205692b14d 46 if((con.buttons[0]>>4)%2) //cross
WAT34 16:5f205692b14d 47 angle = 45;
WAT34 16:5f205692b14d 48 if((con.buttons[0]>>5)%2) //circle
WAT34 16:5f205692b14d 49 angle = 25;
WAT34 16:5f205692b14d 50 if((con.buttons[0]>>6)%2) //triangle
WAT34 13:d01356a29b93 51 angle = 10;
WAT34 13:d01356a29b93 52 if((con.buttons[0]>>13)%2 )
WAT34 13:d01356a29b93 53 {
WAT34 13:d01356a29b93 54 if(!prevSwing)
WAT34 13:d01356a29b93 55 fishingPole.toggleSwing();
WAT34 13:d01356a29b93 56 prevSwing = true;
WAT34 13:d01356a29b93 57 }else{
WAT34 13:d01356a29b93 58 prevSwing = false;
WAT34 13:d01356a29b93 59 }
WAT34 12:9d21189a8eb0 60 if(con.buttons[3] < 117 )
WAT34 7:baf16d0c14e7 61 {
WAT34 7:baf16d0c14e7 62 if(!prevAnglechange)
WAT34 13:d01356a29b93 63 angle += 5;
WAT34 7:baf16d0c14e7 64 prevAnglechange =true;
WAT34 12:9d21189a8eb0 65 }else
WAT34 12:9d21189a8eb0 66 if(con.buttons[3] > 137)
WAT34 7:baf16d0c14e7 67 {
WAT34 7:baf16d0c14e7 68 if(!prevAnglechange)
WAT34 13:d01356a29b93 69 angle -= 5;
WAT34 7:baf16d0c14e7 70 prevAnglechange =true;
WAT34 7:baf16d0c14e7 71 }else{
WAT34 7:baf16d0c14e7 72 prevAnglechange =false;
WAT34 7:baf16d0c14e7 73 }
WAT34 6:0bee4b2bb400 74 if(angle < 5){
WAT34 6:0bee4b2bb400 75 angle = 5;
WAT34 6:0bee4b2bb400 76 }
WAT34 6:0bee4b2bb400 77 if(angle > 80 ){
WAT34 6:0bee4b2bb400 78 angle = 80;
WAT34 6:0bee4b2bb400 79 }
WAT34 6:0bee4b2bb400 80
WAT34 13:d01356a29b93 81 if((con.buttons[0] >> 9)%2)
WAT34 13:d01356a29b93 82 turnRate = 127+127*.2; //turn at .2 power
WAT34 13:d01356a29b93 83 else if((con.buttons[0] >> 10)%2)
WAT34 13:d01356a29b93 84 turnRate = 127-127*.2;
WAT34 12:9d21189a8eb0 85 else
WAT34 12:9d21189a8eb0 86 turnRate = 127;
WAT34 12:9d21189a8eb0 87
WAT34 13:d01356a29b93 88 if((con.buttons[0]>>14)%2)
WAT34 13:d01356a29b93 89 angling = 0.4;
WAT34 13:d01356a29b93 90 else if((con.buttons[0]>>12)%2)
WAT34 13:d01356a29b93 91 angling = -0.4;
WAT34 13:d01356a29b93 92 else
WAT34 13:d01356a29b93 93 angling = 0;
WAT34 13:d01356a29b93 94 fishingPole.setAnglingSpeed(angling);
WAT34 13:d01356a29b93 95
WAT34 13:d01356a29b93 96 if((con.buttons[0]>>15)%2)
WAT34 13:d01356a29b93 97 fishingPole.Deploy();
WAT34 13:d01356a29b93 98
WAT34 16:5f205692b14d 99
WAT34 13:d01356a29b93 100 fishingPole.update();
WAT34 13:d01356a29b93 101
WAT34 12:9d21189a8eb0 102
WAT34 6:0bee4b2bb400 103 boom.setTargetArmAngle(angle);
WAT34 6:0bee4b2bb400 104 led = boom.beltSpeed;
WAT34 12:9d21189a8eb0 105 boom.move(con.buttons[1],con.buttons[2],turnRate);
WAT34 4:c2b09fa31492 106 boom.update();
WAT34 16:5f205692b14d 107
WAT34 16:5f205692b14d 108 serial.printf("controller:%d",con.buttons[0]);
WAT34 6:0bee4b2bb400 109 wait_ms(20);
WAT34 5:9c52e6c14c87 110 con.update();
WAT34 4:c2b09fa31492 111
WAT34 4:c2b09fa31492 112 }
WAT34 4:c2b09fa31492 113
WAT34 1:35e874e10602 114 }