File content as of revision 14:5cc44bec9cfc:

#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);
PwmOut led(LED1);
DigitalIn button(BUTTON1);
boomerang boom(&rs485);
gakugaku fishingPole(&rs485);
recieveController con(PC_12,PD_2,198);

void init()
{
  button.mode(PullUp);

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

int main() {
  init();
  int angle = 10,turnRate = 127;
  double angling;
  bool prevAnglechange = false,prevSwing=false;
  char data[20],*data2,*data3;
  // commThread.start(&comm);
  // ticker.attach(&updater,0.01);
  while(true)
  {
    if((con.buttons[0] >>7)%2 )
      boom.fire();

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

    if((con.buttons[0]>>6)%2)
      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 += 5;
      prevAnglechange =true;
    }else
    if(con.buttons[3] > 137)
    {
      if(!prevAnglechange)
        angle -= 5;
      prevAnglechange =true;
    }else{
      prevAnglechange =false;
    }
    if(angle < 5){
      angle = 5;
    }
    if(angle > 80 ){
      angle = 80;
    }

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

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

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

    fishingPole.update();


    boom.setTargetArmAngle(angle);
    led = boom.beltSpeed;
    boom.move(con.buttons[1],con.buttons[2],turnRate);
    boom.update();
    printf("PID input:%d  PID result %d\n",angle,con.buttons[0]);
    wait_ms(20);
      /* code */
     con.update();
    // printf("whats up\n" );

  }

}