main.cpp

00001 #include "mbed.h"
00002 #include "hayatoBoomerang.h"
00003 #include "recieveController.h"
00004 #include "omni.h"
00005 #include "gakugaku.h"
00006 
00007 
00008 Serial rs485(PC_10,PC_11,38400);
00009 Serial serial(USBTX,USBRX);
00010 Serial esp(PA_9,PA_10,115200);
00011 Serial spiral(PA_0,PA_1,38400);
00012 PwmOut led(LED1);
00013 DigitalIn button(BUTTON1);
00014 boomerang boom(&rs485,&spiral);
00015 gakugaku fishingPole(&spiral);
00016 Ticker debugesp,ticker;
00017 recieveController con(PC_12,PD_2,198);
00018 BusIn modeChange(PA_4,PB_0,PC_2,PC_1);
00019 BusOut modeStatus(NC,NC,NC,NC);
00020 BusOut angleState(NC,NC,NC);
00021 
00022 double angle = 10;
00023 
00024 
00025 void update()
00026 {
00027   boom.update();
00028   fishingPole.update();
00029 }
00030 void init()
00031 {
00032   button.mode(PullUp);
00033 
00034  serial.baud(115200);
00035   //boom.calibrateArm();
00036   boom.setTargetMotorSpeed(0);
00037 }
00038 
00039 void debugout()
00040 {
00041   esp.printf("angle:%f\n",angle);
00042 }
00043 
00044 int main() {
00045   init();
00046   double turnRate = 0;
00047   double flywheelspeed;
00048   double angling;
00049   bool prevAnglechange = false,prevSwing=false;
00050     char data[20],*data2,*data3;
00051   modeChange.mode(PullUp);
00052   if((15-modeChange.read())%3 == 0)
00053     flywheelspeed =13000;
00054   else if(((15-modeChange.read())%3) == 1)
00055     flywheelspeed =13000*3.0/4.0;
00056   else if(((15-modeChange.read())%3) == 2)
00057     flywheelspeed = 13000*2.0/3.0;
00058   //debugesp.attach(&debugout,3);
00059   // commThread.start(&comm);
00060   while(true)
00061   {
00062     modeStatus = 15-modeChange.read();
00063     if((con.buttons[0] >>7)%2 )
00064     {
00065       boom.fire();
00066     }
00067 
00068     if((con.buttons[0] >>8)%2)
00069       boom.setTargetMotorSpeed(flywheelspeed);
00070 
00071     else
00072       boom.setTargetMotorSpeed(0);
00073 
00074 
00075     if((con.buttons[0]>>3)%2) //square
00076       angle = 80;
00077     if((con.buttons[0]>>4)%2) //cross
00078       angle = 37.5;
00079     if((con.buttons[0]>>5)%2) //circle
00080       angle = 25;
00081     if((con.buttons[0]>>6)%2) //triangle
00082       angle = 10;
00083     if((con.buttons[0]>>13)%2 )
00084     {
00085       if(!prevSwing)
00086         fishingPole.toggleSwing();
00087       prevSwing = true;
00088     }else{
00089       prevSwing = false;
00090     }
00091     if(con.buttons[3] < 117 )
00092     {
00093       if(!prevAnglechange)
00094         angle += 2.5;
00095       prevAnglechange =true;
00096     }else
00097     if(con.buttons[3] > 137)
00098     {
00099       if(!prevAnglechange)
00100         angle -= 2.5;
00101       prevAnglechange =true;
00102     }else{
00103       prevAnglechange =false;
00104     }
00105     if(angle < 5){
00106       angle = 5;
00107     }
00108     if(angle > 90 ){
00109       angle = 90;
00110     }
00111 
00112     if((con.buttons[0] >> 9)%2)
00113       turnRate =-.2; //turn at .2 power
00114     else if((con.buttons[0] >> 10)%2)
00115       turnRate = .2;
00116     else
00117       turnRate = 0;
00118 
00119     if((con.buttons[0]>>12)%2)
00120       angling = -0.2;
00121     else if((con.buttons[0]>>14)%2)
00122       angling = 0.4;
00123     else
00124       angling = 0;
00125     fishingPole.setAnglingSpeed(angling);
00126 
00127     if((con.buttons[0]>>15)%2)
00128       fishingPole.Deploy();
00129 
00130     update();
00131 
00132     boom.setTargetArmAngle(angle);
00133     angleState = int(angle/90.0*7);
00134     led = boom.beltSpeed;
00135     boom.move(con.buttons[1],con.buttons[2],turnRate);
00136     serial.printf("controller:%d\n\r",con.buttons[0]);
00137      con.update();
00138 
00139   }
00140 
00141 }