hoverbrett/controller.ino

111 lines
2.5 KiB
C++

//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();
}
}