//https://github.com/rogerclarkmelbourne/Arduino_STM32   in arduino/hardware
//Board: Generic STM32F103C series
//Upload method: serial
//20k RAM 64k Flash

// RX ist A10, TX ist A9   (3v3 level)
//to flash set boot0 (the one further away from reset button) to 1 and press reset, flash, program executes immediately
//set boot0 back to 0 to run program on powerup

#define PIN_LED PC13

#define PIN_POTI1 PA7
#define PIN_POTI2 PA6

#define SENDPERIOD 50


uint16_t poti1=0;
uint16_t poti2=0;


long last_send=0;

int16_t out_steer=0;
int16_t out_speed=0;

uint16_t ch1_in;
uint16_t ch2_in;
uint16_t ch3_in;
uint16_t ch4_in;

//RC Input Pins
#define PIN_CH1 PB9
#define PIN_CH2 PB8
#define PIN_CH3 PB7
#define PIN_CH4 PB6


void setup() {

  Serial.begin(115200); //Debug and Program. A9=TX1,  A10=RX1  (3v3 level)

  Serial2.begin(19200); //control.  B10=TX3, B11=RX3 (Serial2 is Usart 3)

  
  pinMode(PIN_LED, OUTPUT);
  digitalWrite(PIN_LED,LOW);

  pinMode(PIN_POTI1, INPUT);
  pinMode(PIN_POTI2, INPUT);


  pinMode(PIN_CH1, INPUT);
  pinMode(PIN_CH2, INPUT);
  pinMode(PIN_CH3, INPUT);
  pinMode(PIN_CH4, INPUT);

}

void loop() {

  //RC Input.  chn_in between 0 and 1000
  ch1_in = constrain(map(pulseIn(PIN_CH1, HIGH, 25000),1000,2000, 0,1000), 0,1000); //Read Pulse Width. (pulseIn values typically between 1000 and 2000). Map and constrain between 0 and 1000
  ch2_in = constrain(map(pulseIn(PIN_CH2, HIGH, 25000),1000,2000, 0,1000), 0,1000);
  //ch3_in = constrain(map(pulseIn(PIN_CH3, HIGH, 25000),1000,2000, 0,1000), 0,1000);
  //ch4_in = constrain(map(pulseIn(PIN_CH4, HIGH, 25000),1000,2000, 0,1000), 0,1000);
  //ch1 = steer (ail)
  //ch2 = speed (ele)
  //ch3 = speed multiplier (thr)

  //Potis
  /*poti1=analogRead(PIN_POTI1);
  #define POTIMIN 100
  #define POTIMAX 4000
  if (poti1<POTIMIN){
    poti1=POTIMIN;
  }else if(poti1>POTIMAX){
    poti1=POTIMAX;
  }
  poti1=(poti1-POTIMIN)*1000/(POTIMAX-POTIMIN);

  poti2=analogRead(PIN_POTI2);
  if (poti2<POTIMIN){
    poti2=POTIMIN;
  }else if(poti2>POTIMAX){
    poti2=POTIMAX;
  }
  poti2=(poti2-POTIMIN)*1000/(POTIMAX-POTIMIN);
  
  out_speed=poti1;
  out_steer=0;
  */

  //out_speed=(int16_t)( (ch2_in*2-1000)*(ch3_in/1000.0) );
  out_speed=(int16_t)( (ch2_in*2-1000));
  out_steer=ch1_in*2-1000;

  if (millis()-last_send>SENDPERIOD){
    Serial2.write((uint8_t *) &out_steer, sizeof(out_steer)); 
    Serial2.write((uint8_t *) &out_speed, sizeof(out_speed));
    last_send=millis();

    Serial.print("Steer=");
    Serial.println(out_steer);
    Serial.print("Speed=");
    Serial.println(out_speed);
    Serial.println();
  }

}