File content as of revision 24:050d78bb5b69:

#include "mbed.h"
#include "hayatoBoomerang.h"
#include "recieveController.h"
#include "omni.h"
#include "gakugaku.h"


Serial rs485(PC_10,PC_11,38400);
Serial serial(USBTX,USBRX);
Serial esp(PA_9,PA_10,115200);
Serial spiral(PA_0,PA_1,38400);
PwmOut led(LED1);
DigitalIn button(BUTTON1);
boomerang boom(&rs485,&spiral);
gakugaku fishingPole(&spiral);
Ticker debugesp,ticker;
recieveController con(PC_12,PD_2,198);
BusIn modeChange(PA_4,PB_0,PC_2,PC_1);
BusOut modeStatus(NC,NC,NC,NC);
BusOut angleState(NC,NC,NC);

double angle = 10;


void update()
{
  boom.update();
  fishingPole.update();
}
void init()
{
  button.mode(PullUp);

 serial.baud(115200);
  //boom.calibrateArm();
  boom.setTargetMotorSpeed(0);
}

void debugout()
{
  esp.printf("angle:%f\n",angle);
}

int main() {
  init();
  double turnRate = 0;
  double flywheelspeed;
  double angling;
  bool prevAnglechange = false,prevSwing=false;
    char data[20],*data2,*data3;
  modeChange.mode(PullUp);
  if((15-modeChange.read())%3 == 0)
    flywheelspeed =13000;
  else if(((15-modeChange.read())%3) == 1)
    flywheelspeed =13000*3.0/4.0;
  else if(((15-modeChange.read())%3) == 2)
    flywheelspeed = 13000*2.0/3.0;
  //debugesp.attach(&debugout,3);
  // commThread.start(&comm);
  while(true)
  {
    modeStatus = 15-modeChange.read();
    if((con.buttons[0] >>7)%2 )
    {
      boom.fire();
    }

    if((con.buttons[0] >>8)%2)
      boom.setTargetMotorSpeed(flywheelspeed);

    else
      boom.setTargetMotorSpeed(0);


    if((con.buttons[0]>>3)%2) //square
      angle = 80;
    if((con.buttons[0]>>4)%2) //cross
      angle = 45;
    if((con.buttons[0]>>5)%2) //circle
      angle = 25;
    if((con.buttons[0]>>6)%2) //triangle
      angle = 10;
    if((con.buttons[0]>>13)%2 )
    {
      if(!prevSwing)
        fishingPole.toggleSwing();
      prevSwing = true;
    }else{
      prevSwing = false;
    }
    if(con.buttons[3] < 117 )
    {
      if(!prevAnglechange)
        angle += 2.5;
      prevAnglechange =true;
    }else
    if(con.buttons[3] > 137)
    {
      if(!prevAnglechange)
        angle -= 2.5;
      prevAnglechange =true;
    }else{
      prevAnglechange =false;
    }
    if(angle < 5){
      angle = 5;
    }
    if(angle > 90 ){
      angle = 90;
    }

    if((con.buttons[0] >> 9)%2)
      turnRate =-.2; //turn at .2 power
    else if((con.buttons[0] >> 10)%2)
      turnRate = .2;
    else
      turnRate = 0;

    if((con.buttons[0]>>14%2))
      angling = -0.4;
    else if((con.buttons[0])%2)
      angling = 0.2;
    else
      angling = 0;
    fishingPole.setAnglingSpeed(angling);

    if((con.buttons[0]>>15)%2)
      fishingPole.Deploy();

    update();

    boom.setTargetArmAngle(angle);
    angleState = int(angle/90.0*7);
    led = boom.beltSpeed;
    boom.move(con.buttons[1],con.buttons[2],turnRate);
    serial.printf("controller:%d\n\r",con.buttons[0]);
     con.update();

  }

}