NagaokaRoboticsClub_mbedTeam / Mbed OS hayatoShooter

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

Dependencies:   PID QEI ikarashiMDC recieveController omni

main.cpp@14:5cc44bec9cfc, 2017-10-05 (annotated)

Committer:
WAT34
Date:
Thu Oct 05 16:09:28 2017 +0900
Revision:
14:5cc44bec9cfc
Parent:
13:d01356a29b93
Parent:
9:6540a10a07fb
Child:
16:5f205692b14d
merged

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 6:0bee4b2bb400 23 boom.setTargetArmAngle(40);
WAT34 4:c2b09fa31492 24 }
WAT34 1:35e874e10602 25
WAT34 1:35e874e10602 26 int main() {
WAT34 4:c2b09fa31492 27 init();
WAT34 13:d01356a29b93 28 int angle = 10,turnRate = 127;
WAT34 13:d01356a29b93 29 double angling;
WAT34 13:d01356a29b93 30 bool prevAnglechange = false,prevSwing=false;
WAT34 5:9c52e6c14c87 31 char data[20],*data2,*data3;
WAT34 5:9c52e6c14c87 32 // commThread.start(&comm);
WAT34 6:0bee4b2bb400 33 // ticker.attach(&updater,0.01);
WAT34 4:c2b09fa31492 34 while(true)
WAT34 4:c2b09fa31492 35 {
WAT34 13:d01356a29b93 36 if((con.buttons[0] >>7)%2 )
WAT34 6:0bee4b2bb400 37 boom.fire();
WAT34 6:0bee4b2bb400 38
WAT34 13:d01356a29b93 39 if((con.buttons[0] >>8)%2)
WAT34 13:d01356a29b93 40 boom.setTargetMotorSpeed(13000);
WAT34 6:0bee4b2bb400 41 else
WAT34 6:0bee4b2bb400 42 boom.setTargetMotorSpeed(0);
WAT34 6:0bee4b2bb400 43
WAT34 13:d01356a29b93 44 if((con.buttons[0]>>6)%2)
WAT34 13:d01356a29b93 45 angle = 10;
WAT34 13:d01356a29b93 46
WAT34 13:d01356a29b93 47 if((con.buttons[0]>>13)%2 )
WAT34 13:d01356a29b93 48 {
WAT34 13:d01356a29b93 49 if(!prevSwing)
WAT34 13:d01356a29b93 50 fishingPole.toggleSwing();
WAT34 13:d01356a29b93 51 prevSwing = true;
WAT34 13:d01356a29b93 52 }else{
WAT34 13:d01356a29b93 53 prevSwing = false;
WAT34 13:d01356a29b93 54 }
WAT34 12:9d21189a8eb0 55 if(con.buttons[3] < 117 )
WAT34 7:baf16d0c14e7 56 {
WAT34 7:baf16d0c14e7 57 if(!prevAnglechange)
WAT34 13:d01356a29b93 58 angle += 5;
WAT34 7:baf16d0c14e7 59 prevAnglechange =true;
WAT34 12:9d21189a8eb0 60 }else
WAT34 12:9d21189a8eb0 61 if(con.buttons[3] > 137)
WAT34 7:baf16d0c14e7 62 {
WAT34 7:baf16d0c14e7 63 if(!prevAnglechange)
WAT34 13:d01356a29b93 64 angle -= 5;
WAT34 7:baf16d0c14e7 65 prevAnglechange =true;
WAT34 7:baf16d0c14e7 66 }else{
WAT34 7:baf16d0c14e7 67 prevAnglechange =false;
WAT34 7:baf16d0c14e7 68 }
WAT34 6:0bee4b2bb400 69 if(angle < 5){
WAT34 6:0bee4b2bb400 70 angle = 5;
WAT34 6:0bee4b2bb400 71 }
WAT34 6:0bee4b2bb400 72 if(angle > 80 ){
WAT34 6:0bee4b2bb400 73 angle = 80;
WAT34 6:0bee4b2bb400 74 }
WAT34 6:0bee4b2bb400 75
WAT34 13:d01356a29b93 76 if((con.buttons[0] >> 9)%2)
WAT34 13:d01356a29b93 77 turnRate = 127+127*.2; //turn at .2 power
WAT34 13:d01356a29b93 78 else if((con.buttons[0] >> 10)%2)
WAT34 13:d01356a29b93 79 turnRate = 127-127*.2;
WAT34 12:9d21189a8eb0 80 else
WAT34 12:9d21189a8eb0 81 turnRate = 127;
WAT34 12:9d21189a8eb0 82
WAT34 13:d01356a29b93 83 if((con.buttons[0]>>14)%2)
WAT34 13:d01356a29b93 84 angling = 0.4;
WAT34 13:d01356a29b93 85 else if((con.buttons[0]>>12)%2)
WAT34 13:d01356a29b93 86 angling = -0.4;
WAT34 13:d01356a29b93 87 else
WAT34 13:d01356a29b93 88 angling = 0;
WAT34 13:d01356a29b93 89 fishingPole.setAnglingSpeed(angling);
WAT34 13:d01356a29b93 90
WAT34 13:d01356a29b93 91 if((con.buttons[0]>>15)%2)
WAT34 13:d01356a29b93 92 fishingPole.Deploy();
WAT34 13:d01356a29b93 93
WAT34 13:d01356a29b93 94 fishingPole.update();
WAT34 13:d01356a29b93 95
WAT34 12:9d21189a8eb0 96
WAT34 6:0bee4b2bb400 97 boom.setTargetArmAngle(angle);
WAT34 6:0bee4b2bb400 98 led = boom.beltSpeed;
WAT34 12:9d21189a8eb0 99 boom.move(con.buttons[1],con.buttons[2],turnRate);
WAT34 4:c2b09fa31492 100 boom.update();
WAT34 13:d01356a29b93 101 printf("PID input:%d PID result %d\n",angle,con.buttons[0]);
WAT34 6:0bee4b2bb400 102 wait_ms(20);
WAT34 5:9c52e6c14c87 103 /* code */
WAT34 5:9c52e6c14c87 104 con.update();
WAT34 5:9c52e6c14c87 105 // printf("whats up\n" );
WAT34 4:c2b09fa31492 106
WAT34 4:c2b09fa31492 107 }
WAT34 4:c2b09fa31492 108
WAT34 1:35e874e10602 109 }