hoverbrett/controller.ino

202 lines
5.1 KiB
Arduino
Raw Normal View History

//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
2019-03-16 19:02:27 +00:00
#define DEBUG
#define PIN_LED PC13
#define SENDPERIOD 20
2019-03-16 19:02:27 +00:00
//from left to right. pins at bottom. chips on top
//1 GND (black)
//2 Data
//3 Clock
//4 Reset
//5 +5V (red)
//6 Right BTN
//7 Middle BTN
//8 Left BTN
//pinout: https://martin-prochnow.de/projects/thinkpad_keyboard
//see also https://github.com/feklee/usb-trackpoint/blob/master/code/code.ino
2019-03-16 19:02:27 +00:00
#include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"
RF24 radio(PB0,PB1); //ce, cs
//SCK D13 (Pro mini), A5 (bluepill)
//Miso D12 (Pro mini), A6 (bluepill)
//Mosi D11 (Pro mini), A7 (bluepill)
// Radio pipe addresses for the 2 nodes to communicate.
const uint64_t pipes[2] = { 0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL };
2019-03-16 19:02:27 +00:00
struct nrfdata {
uint8_t steer;
uint8_t speed;
uint8_t commands; //bit 0 set = motor enable
uint8_t checksum;
};
2019-03-16 19:02:27 +00:00
nrfdata lastnrfdata;
long last_nrfreceive=0; //last time values were received and checksum ok
long nrf_delay=0;
#define MAX_NRFDELAY 50
2019-03-16 19:02:27 +00:00
//command variables
boolean motorenabled=false; //set by nrfdata.commands
2019-03-16 19:02:27 +00:00
long last_send=0;
int16_t out_steer=0; //between -1000 and 1000
int16_t out_speed=0;
uint8_t out_checksum=0; //0= disable motors, 255=reserved, 1<=checksum<255
#define NRFDATA_CENTER 127
2019-03-16 19:02:27 +00:00
boolean armed=false;
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);
2019-03-16 19:02:27 +00:00
digitalWrite(PIN_LED,HIGH);
2019-03-16 19:02:27 +00:00
Serial.println("Initializing nrf24");
radio.begin();
radio.setRetries(15,15); // optionally, increase the delay between retries & # of retries
radio.setPayloadSize(8); // optionally, reduce the payload size. seems to improve reliability
radio.openWritingPipe(pipes[0]); //write on pipe 0
radio.openReadingPipe(1,pipes[1]); //read on pipe 1
radio.startListening();
Serial.println("Initialized");
}
void loop() {
2019-03-16 19:02:27 +00:00
//NRF24
nrf_delay=millis()-last_nrfreceive; //update nrf delay
if ( radio.available() )
{
//Serial.println("radio available ...");
bool done = false;
while (!done)
{
digitalWrite(PIN_LED, !digitalRead(PIN_LED));
done = radio.read( &lastnrfdata, sizeof(nrfdata) );
//parse commands
motorenabled = (lastnrfdata.commands & (1 << 0)); //check bit 0
if (!motorenabled){ //disable motors?
armed=false;
Serial.println("!motorenebled. armed=false");
}
if (lastnrfdata.speed==NRFDATA_CENTER && lastnrfdata.speed==NRFDATA_CENTER){ //arm only when centered
armed=true; //arm at first received packet
Serial.println("centered. armed=true");
}
uint8_t calcchecksum=(uint8_t)((lastnrfdata.steer+3)*(lastnrfdata.speed+13));
if (lastnrfdata.checksum!=calcchecksum){ //checksum not ok?
armed=false;
Serial.println("Checksum fail. armed=false");
}else{ //checksum ok
last_nrfreceive=millis();
}
#ifdef DEBUG
Serial.print("Received:");
Serial.print(" st=");
Serial.print(lastnrfdata.steer);
Serial.print(", sp=");
Serial.print(lastnrfdata.speed);
Serial.print(", c=");
Serial.print(lastnrfdata.commands);
Serial.print(", chks=");
Serial.print(lastnrfdata.checksum);
Serial.print("nrfdelay=");
Serial.print(nrf_delay);
#endif
//y positive = forward
//x positive = right
}
}
if (armed && nrf_delay>=MAX_NRFDELAY){ //too long since last sucessful nrf receive
armed=false;
Serial.println("too long since last nrf data. armed=false");
}
if (armed){
//out_speed=(int16_t)( (lastnrfdata.y-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
//out_steer=(int16_t)( -(lastnrfdata.x-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
out_speed=(int16_t)( ((int16_t)(lastnrfdata.speed)-NRFDATA_CENTER)*1000/127 );
out_steer=(int16_t)( ((int16_t)(lastnrfdata.steer)-NRFDATA_CENTER)*1000/127 );
}else{ //took too long since last nrf data
out_steer=0;
out_speed=0;
}
if (millis()-last_send>SENDPERIOD){
2019-03-16 19:02:27 +00:00
//calculate checksum
out_checksum = ((uint8_t) ((uint8_t)out_steer)*((uint8_t)out_speed)); //simple checksum
if (out_checksum==0 || out_checksum==255){ out_checksum=1; } //cannot be 0 or 255 (special purpose)
if (!motorenabled){ //disable motors?
out_checksum=0; //checksum=0 disables motors
}
Serial2.write((uint8_t *) &out_steer, sizeof(out_steer));
Serial2.write((uint8_t *) &out_speed, sizeof(out_speed));
2019-03-16 19:02:27 +00:00
Serial2.write((uint8_t *) &out_checksum, sizeof(out_checksum));
last_send=millis();
#ifdef DEBUG
2019-03-16 19:02:27 +00:00
Serial.print(" steer=");
Serial.print(out_steer);
Serial.print(" speed=");
Serial.print(out_speed);
2019-03-16 19:02:27 +00:00
Serial.print(" checksum=");
Serial.print(out_checksum);
Serial.println();
2019-03-16 19:02:27 +00:00
#endif
}
}