interfisch
/
nippleremote
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
nippleremote/nonpio_nippleremote_firmware/nippleremote_firmware.ino

536 lines
14 KiB
C++
Raw Blame History

//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
//Arduino Pro Mini 328P 5V 16MHz
//hold power button pressed during flashing
//pinout: https://martin-prochnow.de/projects/thinkpad_keyboard
//see also https://github.com/feklee/usb-trackpoint/blob/master/code/code.ino
//#define DEBUG
#include "Trackpoint.h"
//Default:
/*Trackpoint trackpoint(8, // CLK
9, // DATA
12); // RESET
*/
/*funktioniert
* Trackpoint trackpoint(2, // CLK (rosa)
3, // DATA (gelb)
4); // RESET (gruen)
*/
Trackpoint trackpoint(3, // CLK (rosa, TP3)
4, // DATA (gelb, TP2)
2); // RESET (gruen, TP4)
#include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"
#include "printf.h"
bool radioOk=false; //true, if sending was successfull. can be false, even if data was send and received
RF24 radio(9,10); //ce, cs
//SCK D13
//Miso D12
//Mosi D11
// Radio pipe addresses for the 2 nodes to communicate.
const uint64_t pipes[2] = { 0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL }; //0xF0F0F0F0xxLL, xx is 1-byte address
#define NRF24CHANNEL 75
struct nrfdata {
uint8_t steer; //between 0 and 255, 127 is stop. will be scaled to -1000 to 1000
uint8_t speed; //between 0 and 255, 127 is stop. will be scaled to -1000 to 1000
uint8_t commands; //bit 0 set = motor enable
uint8_t checksum;
};
long last_sendNRF=0;
#define NRFSEND_DELAY 20 //ms
#define PIN_TOUCH 5
long last_touch=0;
#define TOUCH_TIMEOUT 100
//command variables
boolean motorenabled=false;
#define PIN_LED A1
#define PIN_BUTTON 6
#define PIN_POWERON 7
#define TRACKPOINT_MAX 70 //value for maximum stick movement
float speedscale=0.0;
float steerscale=0.0;
int16_t last_xin=0;
int16_t last_yin=0;
int16_t xin_smooth=0;
int16_t yin_smooth=0;
int16_t maxacc=0;
int16_t maxacc_brake=0;
int16_t maxaccsteer=0;
int16_t maxaccsteer_brake=0;
#define SETUP_NONE 0
#define SETUP_WAIT 1 //waiting for input
#define SETUP_DONE 2 //waiting after input (do not move motors)
uint8_t setupmode=SETUP_NONE;
long setupmode_waitstarttime=0; //starttime of SETUP_WAIT mode
#define SETUP_WAIT_TIMEOUT 10000 //maximum time to wait for input before canceling
#define SETUP_HOLD_POWEROFF 2000 //if button held down after x ms in setup mode, power off
#define SETUP_DONE_TIME 1000 //time to keep motors disabled after exiting setup
#define SETUP_MOVE_THRESHOLD 275 //500*TRACKPOINT_MAX/127
uint8_t speedmode=1; //0 (slow), 1(medium), 2(fast)
#define SETUP_SPEEDMODE_MAX 2
uint16_t led_ton=0; //never. time in ms for on time
uint16_t led_toff=65535; //always
long led_nextswitch=0;
#define TIME_INACTIVITY_POWEROFF 120000
long time_lastactivity=0;
#define ACTIVITYMOVEMENT 5//Stick movement for activity recognition
boolean touching=false;
int voltage=4000;
#define VOLTAGE_WARN 3400
/*
* 3681=3.725V
*/
void setup() {
//Mouse.begin();
pinMode(PIN_TOUCH, INPUT_PULLUP);
pinMode(PIN_LED, OUTPUT);
pinMode(PIN_BUTTON, INPUT_PULLUP);
pinMode(PIN_POWERON, OUTPUT);
digitalWrite(PIN_LED, LOW);
digitalWrite(PIN_POWERON, HIGH); //keep unit powered on
Serial.begin(115200);
printf_begin();
Serial.println("Booting");
radio.begin();
//Serial.print("CRC Length=");
//Serial.println(radio.getCRCLength());
radio.setDataRate( RF24_250KBPS ); //set to slow data rate. default was 1MBPS
//radio.setDataRate( RF24_1MBPS );
radio.setChannel(NRF24CHANNEL); //0 to 124 (inclusive)
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.openWritingPipe(pipes[1]); //write on pipe 1
radio.openReadingPipe(1,pipes[0]); //read on pipe 0
radio.printDetails();
radio.startListening();
#ifdef DEBUG
Serial.println("Radio initialized");
#endif
trackpoint.reset();
trackpoint.setRemoteMode();
trackpoint.setSensitivityFactor(0xc0); // more sensitive than by default
#ifdef DEBUG
Serial.println("Trackpoint initialized");
#endif
voltage=readVcc();
Serial.print("Voltage=");
Serial.println( voltage, DEC );
setup_updateSpeedmode(); //set speeds
}
/*
void sendButtonState(byte state) {
static const char hidStates[] = {MOUSE_LEFT, MOUSE_RIGHT};
for (byte i = 0; i < sizeof(hidStates); i++) {
byte hidState = hidStates[i];
if (state & (1 << i)) {
Mouse.press(hidState);
} else if (Mouse.isPressed(hidState)) {
Mouse.release(hidState);
}
}
}
*/
// Reads TrackPoint data and sends data to computer.
void loop() {
if (millis()-last_sendNRF >= NRFSEND_DELAY)
{
voltage=readVcc(); //read own voltage
last_sendNRF=millis();
trackpoint.readData(); //discard last value. otherwise values are scales way too high
Trackpoint::DataReport d = trackpoint.readData(); //d.x and d.y between 128 to 255=-1000 to 0, and 0 to 127=0 to +1000
#ifdef DEBUG
Serial.print("DataReport: ");
Serial.print(d.x);
Serial.print(", ");
Serial.println(d.y);
#endif
nrfdata senddata;
//senddata.steer=map(constrain((uint8_t)(d.x+127),127-TRACKPOINT_MAX,127+TRACKPOINT_MAX) , 127-TRACKPOINT_MAX,127+TRACKPOINT_MAX, 127+(127*steerscale), 127-(127*steerscale) ); //steer
//senddata.speed=map(constrain((uint8_t)(d.y+127),127-TRACKPOINT_MAX,127+TRACKPOINT_MAX) , 127-TRACKPOINT_MAX,127+TRACKPOINT_MAX, 127-(127*speedscale), 127+(127*speedscale) ); //speed
//map x and y to -1000 to 1000
int16_t xin;
if (d.x>=0 && d.x<=127){ //positive range
xin=map(constrain((int16_t)d.x,0,TRACKPOINT_MAX) , 0,TRACKPOINT_MAX, 0, 1000 );
}else{ //negative range 128(=-1000) to 255(0)
xin=map(constrain((int16_t)d.x,127+TRACKPOINT_MAX,255) , 127+TRACKPOINT_MAX,255, -1000, 0 );
}
int16_t yin;
if (d.y>=0 && d.y<=127){ //positive range
yin=map(constrain((float)d.y,0,TRACKPOINT_MAX) , 0,TRACKPOINT_MAX, 0, 1000 );
}else{ //negative range 128(=-1000) to 255(0)
yin=map(constrain((float)d.y,127+TRACKPOINT_MAX,255) , 127+TRACKPOINT_MAX,255, -1000, 0 );
}
last_xin=xin; //save position values for other stuff than control
last_yin=yin;
if (abs(xin)>ACTIVITYMOVEMENT){
time_lastactivity=millis(); //reset activity timeout
}else if (abs(yin)>ACTIVITYMOVEMENT){
time_lastactivity=millis(); //reset activity timeout
}
/*
float r=sqrt((xin*xin) + (yin*yin));
float phi=atan2(yin,xin); // arc tangent of y/x. 0 is right
Serial.print(xin,0);
Serial.print(", ");
Serial.print(yin,0);
Serial.print(": ");
Serial.print(r,0);
Serial.print(", ");
Serial.println(phi,4);*/
//xin_smooth=smoothfilter*xin_smooth + (1-smoothfilter)*xin;
//yin_smooth=smoothfilter*yin_smooth + (1-smoothfilter)*yin;
if (maxaccsteer>0){
// ### X ###
int16_t _xaccel=xin_smooth-xin;
if ((xin_smooth>0 && xin<-2) || (xin_smooth<0 && xin>2) ){ //if actively braking
if (_xaccel<-maxaccsteer_brake){ //limit deceleration
_xaccel=-maxaccsteer_brake;
}else if (_xaccel>maxaccsteer_brake){
_xaccel=maxaccsteer_brake;
}
}else{ //not braking
if (_xaccel<-maxaccsteer){ //limit acceleration
_xaccel=-maxaccsteer;
}else if (_xaccel>maxaccsteer){
_xaccel=maxaccsteer;
}
}
xin_smooth-=_xaccel; //update value
}else{ //no acc limit
xin_smooth=xin; //update immediately
}
if (maxacc>0){
// ### Y ###
int16_t _yaccel=yin_smooth-yin;
if ((yin_smooth>0 && yin<-2) || (yin_smooth<0 && yin>2) ){ //if actively braking
if (_yaccel<-maxacc_brake){ //limit deceleration
_yaccel=-maxacc_brake;
}else if (_yaccel>maxacc_brake){
_yaccel=maxacc_brake;
}
}else{ //not braking
if (_yaccel<-maxacc){ //limit acceleration
_yaccel=-maxacc;
}else if (_yaccel>maxacc){
_yaccel=maxacc;
}
}
yin_smooth-=_yaccel; //update value
}else{ //no acc limit
yin_smooth=yin;
}
senddata.steer=map(xin_smooth, -1000,1000, 127+(128*steerscale), 127-(127*steerscale) ); //steer
senddata.speed=map(yin_smooth, -1000,1000, 127-(127*speedscale), 127+(128*speedscale) ); //speed
senddata.commands=0; //reset
if (!radioOk || setupmode!=SETUP_NONE){ //if last transmission failed or in setup mode
//senddata.steer=127; //stop
//senddata.speed=127;
senddata.commands|= 0 << 0; //motorenabled send false
xin_smooth=0; //reset smooth value
yin_smooth=0;
}else{
senddata.commands|= motorenabled << 0; //motorenabled is bit 0
}
#ifdef DEBUG
Serial.print(senddata.steer);
Serial.print(", ");
Serial.println(senddata.speed);
#endif
senddata.checksum=(uint8_t)((senddata.steer+3)*(senddata.speed+13));
sendRF(senddata);
#ifdef DEBUG
Serial.println( readVcc(), DEC );
#endif
}
if(!digitalRead(PIN_TOUCH)){ //check touch
last_touch=millis();
}
if(millis()-last_touch <= TOUCH_TIMEOUT){ //is touched
if (!touching && setupmode!=SETUP_DONE) { //was false, is touching again (and not during setup_done wait)
Serial.println("touching was false");
if (last_xin==0 && last_yin==0) { //stick at center position
touching=true; //enable only if stick is at center again
motorenabled=true;
Serial.println("touching reactivated");
}
}else{
motorenabled=true;
}
}else{
touching=false;
motorenabled=false;
}
if (!digitalRead(PIN_BUTTON) && setupmode==SETUP_NONE){ //Button pressed, and not in setup mode
setupmode=SETUP_WAIT;
setupmode_waitstarttime=millis();
Serial.println("Entering Setup");
//digitalWrite(PIN_POWERON, LOW); //Power off
}
//inactivity poweroff
if (millis()>time_lastactivity+TIME_INACTIVITY_POWEROFF){
Serial.println("Inactivity Poweroff");
delay(100);
digitalWrite(PIN_POWERON, LOW); //Power off
}
switch(setupmode){
case SETUP_WAIT:
if (millis()>setupmode_waitstarttime+SETUP_WAIT_TIMEOUT){ //waittime over
setupmode=SETUP_NONE; //exit setup
}else if(last_yin > SETUP_MOVE_THRESHOLD){ //y moved up
Serial.print("Moved Up");
if (speedmode<SETUP_SPEEDMODE_MAX){ //if not already at maximum
speedmode+=1;
}
setup_updateSpeedmode();
setupmode=SETUP_DONE; //exit setupmode
setupmode_waitstarttime=millis(); //use this value for done timer
}else if(last_yin < -SETUP_MOVE_THRESHOLD){ //y moved down
Serial.print("Moved Down");
if (speedmode>0){ //if not already at minimum
speedmode-=1;
}
setup_updateSpeedmode();
setupmode=SETUP_DONE; //exit setupmode
setupmode_waitstarttime=millis();//use this value for done timer
}else if(last_xin < -SETUP_MOVE_THRESHOLD){ //y moved left
Serial.print("Moved Left");
maxacc=20; //the higher the snappier
maxacc_brake=30;
maxaccsteer=30;
maxaccsteer_brake=70;
setupmode=SETUP_DONE; //exit setupmode
setupmode_waitstarttime=millis();//use this value for done timer
}else if(last_xin > SETUP_MOVE_THRESHOLD){ //y moved right
Serial.print("Moved Right");
maxacc=0;
maxacc_brake=0;
maxaccsteer=0;
maxaccsteer_brake=0;
setupmode=SETUP_DONE; //exit setupmode
setupmode_waitstarttime=millis();//use this value for done timer
}else if (millis()>setupmode_waitstarttime+SETUP_HOLD_POWEROFF && !digitalRead(PIN_BUTTON)){ //if button held down after SETUP_HOLD_POWEROFF
Serial.println("Manual Power Off");
delay(100);
digitalWrite(PIN_POWERON, LOW); //Power off
}
if (!touching){ //remote got put away (not touch)
Serial.println("Poweroff");
delay(100);
digitalWrite(PIN_POWERON, LOW); //Power off
}
break;
case SETUP_DONE:
touching=false;
motorenabled=false;
Serial.println("touching set false");
if (millis()>setupmode_waitstarttime+SETUP_DONE_TIME){
setupmode=SETUP_NONE; //return to control mode, allows enabling motors
}
break;
}
//LED Blink Codes
switch(setupmode){
case SETUP_NONE:
if (radioOk){
if (touching){ //=touching
led_ton=500; //always on
led_toff=0;
}else{
led_ton=1000; //blink slowly regulary
led_toff=1000;
}
}else{ //not connected
if (touching){ //=touching
led_ton=5; //short flash
led_toff=200;
}else{
led_ton=0; //off
led_toff=500;
}
}
if (voltage<=VOLTAGE_WARN){
led_ton=25; //flash on fast
led_toff=75;
}
break;
case SETUP_WAIT:
led_ton=200; //blink fast
led_toff=200;
break;
case SETUP_DONE:
led_ton=20; //blink fast
led_toff=20;
break;
}
if (millis()>=led_nextswitch){ //Set LED State by timings
if (digitalRead(PIN_LED)){ //led was on
if (led_toff>0){
digitalWrite(PIN_LED, LOW); //led off
}
led_nextswitch=millis()+led_toff;
}else{
if (led_ton>0){
digitalWrite(PIN_LED, HIGH); //led on
}
led_nextswitch=millis()+led_ton;
}
}
}
void sendRF(nrfdata senddata){
#ifdef DEBUG
Serial.println("Transmitting...");
#endif
radio.stopListening(); //stop listening to be able to transmit
radioOk = radio.write( &senddata, sizeof(nrfdata) );
if (radioOk){
#ifdef DEBUG
Serial.println("ok");
#endif
}else{
#ifdef DEBUG
Serial.println("failed");
#endif
}
radio.startListening();
}
long readVcc() {
long result; // Read 1.1V reference against AVcc
ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
delay(2); // Wait for Vref to settle
ADCSRA |= _BV(ADSC); // Convert
while (bit_is_set(ADCSRA,ADSC));
result = ADCL;
result |= ADCH<<8;
result = 1126400L / result; // Back-calculate AVcc in mV
return result;
}
void setup_updateSpeedmode(){
switch(speedmode){
case 0: //slow
speedscale=0.15;
steerscale=0.2;
break;
case 1: //medium
speedscale=0.4;
steerscale=0.45;
break;
case 2: //fast
speedscale=1.0;
steerscale=0.8;
break;
default:
speedscale=0.1;
steerscale=0.1;
break;
}
}
Reference in New Issue View Git Blame Copy Permalink