Commit d2cc0dae authored by Ricardo Rico Uribe's avatar Ricardo Rico Uribe
Browse files

added arduino code - not tested in real life

parent 43823c5c
//timer setup for timer4
//For arduino Mega
//timer4 will interrupt at 20Hz
//storage variable
const byte INTERRUPT_NOISE_THRESHOLD = 5 ;
const word MOTOR_CENTER = 1000 ;
const word MOTOR_RIGHT = 1300 ;
const word MOTOR_LEFT = 700 ;
const byte MOTOR_THRESHOLD = 10 ;
const word MOTOR_RANGE = MOTOR_RIGHT - MOTOR_LEFT ;
const word RADIO_CENTER = 1500 ;
const word RADIO_RIGHT = 1900 ;
const word RADIO_LEFT = 1100 ;
const byte RADIO_THRESHOLD = 10 ;
const word RADIO_RANGE = RADIO_RIGHT - RADIO_LEFT ;
const int _kp = 1;
const byte interrupt_pin = 2 ;
static volatile uint32_t delta_us ;
static volatile uint32_t prev_change_date_us = 0 ;
static volatile uint32_t pos = 0;
static volatile byte count = 0;
static volatile uint32_t recv = 0;
bool first = true;
bool started = false;
static volatile int scale_pos = 50;
static volatile int scale_radio = 50;
byte speed_command = 0;
//***********************************************************************//
void setup(){
// put your setup code here, to run once:
Serial.begin(9600); //usb pc
Serial1.begin(9600); //sabertooth driver
Serial2.begin(115200); //multiplexer
Serial1.write(speed_command);
while(!Serial){
}
cli(); //stop interrupts
//set timer4 interrupt at 20Hz
TCCR4A = 0;// set entire TCCR4A register to 0
TCCR4B = 0;// same for TCCR4B
TCNT4 = 0;//initialize counter value to 0
// set compare match register for 20hz increments
OCR4A = 311/1;// = (16*10^6) / (20*1024) - 1 (must be <65536) // 780 = 200hz / // turn on CTC mode
TCCR4B |= (1 << WGM12);
// Set CS12 and CS10 bits for 1024 prescaler
TCCR4B |= (1 << CS12) | (1 << CS10);
// enable timer compare interrupt
TIMSK4 |= (1 << OCIE4A);
sei();//allow interrupts
attachInterrupt(digitalPinToInterrupt(interrupt_pin),pwm,CHANGE);
//delay(5);
}//end setup
ISR(TIMER4_COMPA_vect){
//Serial.print("radio: ");
//Serial.println(scale_radio);
Serial2.write(0x43);
count = 0;
first = true;
}
//***********************************************************************//
void loop(){
//do other things here
}
//***********************************************************************//
void pwm() {
//Serial.print("change");
uint32_t curr_date = micros () ;
bool state_us ;
delta_us = curr_date - prev_change_date_us ;
if (delta_us < INTERRUPT_NOISE_THRESHOLD) return ;
/* Record the previous pin state since it has just changed. */
state_us = ! digitalRead (interrupt_pin) ;
/* Ensure that we have detected a falling edge, i.e. the previous level
of the pin was HIGH. */
if (state_us) {
//Serial.print("radio: ");
//Serial.println(delta_us);
if (delta_us <= RADIO_CENTER + RADIO_THRESHOLD && delta_us >= RADIO_CENTER - RADIO_THRESHOLD){
scale_radio = 50;
}
else if (delta_us <= RADIO_LEFT + RADIO_THRESHOLD){
scale_radio = 0;
}
else if (delta_us >= RADIO_RIGHT - RADIO_THRESHOLD){
scale_radio = 100;
} else {
scale_radio = (delta_us - RADIO_LEFT) * 100 / (RADIO_RANGE);
}
//Serial.print("scale_radio: ");
//Serial.println(scale_radio);
}
/* Change the date anyway. */
prev_change_date_us = curr_date ;
}
//***********************************************************************//
void serialEvent2() {
while(Serial2.available()){
if (first) {
recv = Serial2.read();
first = false;
}
else {
recv = (recv << 8) + Serial2.read();
}
count++;
}
if (count == 4) {
pos = recv;
//Serial.print("scale_pos: ");
//Serial.println(pos);
count = 0;
speed_command = 64;
first = true;
if (!started) {
if (pos > MOTOR_CENTER + MOTOR_THRESHOLD || pos < MOTOR_CENTER - MOTOR_THRESHOLD){
if(pos < MOTOR_LEFT) speed_command = 127;
else if(pos > MOTOR_RIGHT) speed_command = 1;
else if(pos < MOTOR_CENTER - MOTOR_THRESHOLD) speed_command = 100;
else if(pos > MOTOR_CENTER + MOTOR_THRESHOLD) speed_command = 28;
} else {
//Serial.print("centered");
speed_command = 64;
started = true;
}
Serial1.write(speed_command);
}
else {
scale_pos = (pos - MOTOR_LEFT)* 100/(MOTOR_RANGE);
//Serial.print("scale_pos: ");
//Serial.println(scale_pos);
int error_pos = scale_radio - scale_pos;
int v = _kp * error_pos;
if (v >= -1 && v <= 1){
speed_command = 64;
}
else if (v >= 30) {
speed_command = 127;
}
else if (v <= -30) {
speed_command = 1;
}
else {
speed_command = (1.26) * (v - 50) + 127;
}
//Serial.print("v: ");
//Serial.println(v);
//Serial.print("speed_command: ");
//Serial.println(speed_command);
}
Serial1.write(speed_command);
}
}
//***********************************************************************//
void serialEvent(){
started = true;
Serial1.write(0);
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment