Commit d2cc0daeab0206e16c51185290942cdad05f8a60

Authored by Ricardo Rico Uribe
1 parent 43823c5c

added arduino code - not tested in real life

Showing 1 changed file with 173 additions and 0 deletions   Show diff stats
arduino_code/adapter_motor/adapter_motor.ino 0 → 100644
... ... @@ -0,0 +1,173 @@
  1 +//timer setup for timer4
  2 +//For arduino Mega
  3 +
  4 +//timer4 will interrupt at 20Hz
  5 +
  6 +//storage variable
  7 +const byte INTERRUPT_NOISE_THRESHOLD = 5 ;
  8 +
  9 +const word MOTOR_CENTER = 1000 ;
  10 +const word MOTOR_RIGHT = 1300 ;
  11 +const word MOTOR_LEFT = 700 ;
  12 +const byte MOTOR_THRESHOLD = 10 ;
  13 +const word MOTOR_RANGE = MOTOR_RIGHT - MOTOR_LEFT ;
  14 +
  15 +const word RADIO_CENTER = 1500 ;
  16 +const word RADIO_RIGHT = 1900 ;
  17 +const word RADIO_LEFT = 1100 ;
  18 +const byte RADIO_THRESHOLD = 10 ;
  19 +const word RADIO_RANGE = RADIO_RIGHT - RADIO_LEFT ;
  20 +
  21 +const int _kp = 1;
  22 +
  23 +const byte interrupt_pin = 2 ;
  24 +static volatile uint32_t delta_us ;
  25 +static volatile uint32_t prev_change_date_us = 0 ;
  26 +static volatile uint32_t pos = 0;
  27 +static volatile byte count = 0;
  28 +static volatile uint32_t recv = 0;
  29 +
  30 +bool first = true;
  31 +bool started = false;
  32 +static volatile int scale_pos = 50;
  33 +static volatile int scale_radio = 50;
  34 +byte speed_command = 0;
  35 +//***********************************************************************//
  36 +void setup(){
  37 +// put your setup code here, to run once:
  38 + Serial.begin(9600); //usb pc
  39 + Serial1.begin(9600); //sabertooth driver
  40 + Serial2.begin(115200); //multiplexer
  41 + Serial1.write(speed_command);
  42 + while(!Serial){
  43 +
  44 + }
  45 + cli(); //stop interrupts
  46 +
  47 + //set timer4 interrupt at 20Hz
  48 + TCCR4A = 0;// set entire TCCR4A register to 0
  49 + TCCR4B = 0;// same for TCCR4B
  50 + TCNT4 = 0;//initialize counter value to 0
  51 + // set compare match register for 20hz increments
  52 + OCR4A = 311/1;// = (16*10^6) / (20*1024) - 1 (must be <65536) // 780 = 200hz / // turn on CTC mode
  53 + TCCR4B |= (1 << WGM12);
  54 + // Set CS12 and CS10 bits for 1024 prescaler
  55 + TCCR4B |= (1 << CS12) | (1 << CS10);
  56 + // enable timer compare interrupt
  57 + TIMSK4 |= (1 << OCIE4A);
  58 +
  59 + sei();//allow interrupts
  60 +
  61 + attachInterrupt(digitalPinToInterrupt(interrupt_pin),pwm,CHANGE);
  62 + //delay(5);
  63 +}//end setup
  64 +
  65 +ISR(TIMER4_COMPA_vect){
  66 + //Serial.print("radio: ");
  67 + //Serial.println(scale_radio);
  68 + Serial2.write(0x43);
  69 + count = 0;
  70 + first = true;
  71 +}
  72 +//***********************************************************************//
  73 +void loop(){
  74 + //do other things here
  75 +}
  76 +//***********************************************************************//
  77 +void pwm() {
  78 + //Serial.print("change");
  79 + uint32_t curr_date = micros () ;
  80 + bool state_us ;
  81 +
  82 + delta_us = curr_date - prev_change_date_us ;
  83 +
  84 + if (delta_us < INTERRUPT_NOISE_THRESHOLD) return ;
  85 + /* Record the previous pin state since it has just changed. */
  86 + state_us = ! digitalRead (interrupt_pin) ;
  87 + /* Ensure that we have detected a falling edge, i.e. the previous level
  88 + of the pin was HIGH. */
  89 + if (state_us) {
  90 + //Serial.print("radio: ");
  91 + //Serial.println(delta_us);
  92 + if (delta_us <= RADIO_CENTER + RADIO_THRESHOLD && delta_us >= RADIO_CENTER - RADIO_THRESHOLD){
  93 + scale_radio = 50;
  94 + }
  95 + else if (delta_us <= RADIO_LEFT + RADIO_THRESHOLD){
  96 + scale_radio = 0;
  97 + }
  98 + else if (delta_us >= RADIO_RIGHT - RADIO_THRESHOLD){
  99 + scale_radio = 100;
  100 + } else {
  101 + scale_radio = (delta_us - RADIO_LEFT) * 100 / (RADIO_RANGE);
  102 + }
  103 + //Serial.print("scale_radio: ");
  104 + //Serial.println(scale_radio);
  105 + }
  106 + /* Change the date anyway. */
  107 + prev_change_date_us = curr_date ;
  108 +}
  109 +//***********************************************************************//
  110 +void serialEvent2() {
  111 + while(Serial2.available()){
  112 + if (first) {
  113 + recv = Serial2.read();
  114 + first = false;
  115 + }
  116 + else {
  117 + recv = (recv << 8) + Serial2.read();
  118 + }
  119 + count++;
  120 + }
  121 + if (count == 4) {
  122 + pos = recv;
  123 + //Serial.print("scale_pos: ");
  124 + //Serial.println(pos);
  125 + count = 0;
  126 + speed_command = 64;
  127 + first = true;
  128 + if (!started) {
  129 + if (pos > MOTOR_CENTER + MOTOR_THRESHOLD || pos < MOTOR_CENTER - MOTOR_THRESHOLD){
  130 + if(pos < MOTOR_LEFT) speed_command = 127;
  131 + else if(pos > MOTOR_RIGHT) speed_command = 1;
  132 + else if(pos < MOTOR_CENTER - MOTOR_THRESHOLD) speed_command = 100;
  133 + else if(pos > MOTOR_CENTER + MOTOR_THRESHOLD) speed_command = 28;
  134 + } else {
  135 + //Serial.print("centered");
  136 + speed_command = 64;
  137 + started = true;
  138 + }
  139 + Serial1.write(speed_command);
  140 + }
  141 + else {
  142 + scale_pos = (pos - MOTOR_LEFT)* 100/(MOTOR_RANGE);
  143 +
  144 + //Serial.print("scale_pos: ");
  145 + //Serial.println(scale_pos);
  146 +
  147 + int error_pos = scale_radio - scale_pos;
  148 + int v = _kp * error_pos;
  149 + if (v >= -1 && v <= 1){
  150 + speed_command = 64;
  151 + }
  152 + else if (v >= 30) {
  153 + speed_command = 127;
  154 + }
  155 + else if (v <= -30) {
  156 + speed_command = 1;
  157 + }
  158 + else {
  159 + speed_command = (1.26) * (v - 50) + 127;
  160 + }
  161 + //Serial.print("v: ");
  162 + //Serial.println(v);
  163 + //Serial.print("speed_command: ");
  164 + //Serial.println(speed_command);
  165 + }
  166 + Serial1.write(speed_command);
  167 + }
  168 +}
  169 +//***********************************************************************//
  170 +void serialEvent(){
  171 + started = true;
  172 + Serial1.write(0);
  173 +}
... ...