diff --git a/vehicle_interface/main/README.md b/vehicle_interface/main/README.md new file mode 100644 index 00000000..b32dfd08 --- /dev/null +++ b/vehicle_interface/main/README.md @@ -0,0 +1,43 @@ +# PropBot + +## Function Library + +An I2C library is necessary for this project. + +Please download the SoftI2CMaster.h from the following github link: https://github.com/felias-fogg/SoftI2CMaster + +And install the library onto Arduino IDE. + +For how to install library on Arduino IDE: https://www.arduino.cc/en/Guide/Libraries + +Please also download UCMotor.h library file and UCMotor.cpp file from https://github.com/UCTRONICS/Smart-Robot-Car-Arduino/tree/master/UCTRONICS_Smart_Robot_Car + +## Remote Controller (FS-T6) + +### Left Joystick + +Forward: Left sets of wheels rotate forward + +Backward: Left sets of wheels rotate backward + +### Right Joystick + +Forward: Right sets of wheels rotate forward + +Backward: Right sets of wheels rotate backward + +### Switch A + +0: E-stop deactivated + +1: E-stop activated, the car will shut down + +### Switch B + +0: Manual control mode + +1: Autonomy mode + +## Binding RC receiver with transmitter + +Plesae refer to this tutorial video: https://www.youtube.com/watch?v=7bX7M6E8U0M diff --git a/vehicle_interface/main/UCMotor.cpp b/vehicle_interface/main/UCMotor.cpp deleted file mode 100644 index d3e6501a..00000000 --- a/vehicle_interface/main/UCMotor.cpp +++ /dev/null @@ -1,729 +0,0 @@ -/** - * @file UCMotor.cpp - * @author - * @brief This was fetched from https://github.com/UCTRONICS/Smart-Robot-Car-Arduino/tree/master/UCTRONICS_Smart_Robot_Car - * - * @note No edits have been made to to this file aside from the brief - * - */ - -// UCTRONOICS Motor shield library -// this code is public domain, enjoy! - - -#if (ARDUINO >= 100) - #include "Arduino.h" -#else - #if defined(__AVR__) - #include - #endif - #include "WProgram.h" -#endif - -#include "UCMotor.h" - - - -static uint8_t latch_state; - -#if (MICROSTEPS == 8) -uint8_t microstepcurve[] = {0, 50, 98, 142, 180, 212, 236, 250, 255}; -#elif (MICROSTEPS == 16) -uint8_t microstepcurve[] = {0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255}; -#endif - -UCMotorController::UCMotorController(void) { - TimerInitalized = false; -} - -void UCMotorController::enable(void) { - // setup the latch - /* - LATCH_DDR |= _BV(LATCH); - ENABLE_DDR |= _BV(ENABLE); - CLK_DDR |= _BV(CLK); - SER_DDR |= _BV(SER); - */ - pinMode(MOTORLATCH, OUTPUT); - pinMode(MOTORENABLE, OUTPUT); - pinMode(MOTORDATA, OUTPUT); - pinMode(MOTORCLK, OUTPUT); - - latch_state = 0; - - latch_tx(); // "reset" - - //ENABLE_PORT &= ~_BV(ENABLE); // enable the chip outputs! - digitalWrite(MOTORENABLE, LOW); -} - - -void UCMotorController::latch_tx(void) { - uint8_t i; - - //LATCH_PORT &= ~_BV(LATCH); - digitalWrite(MOTORLATCH, LOW); - - //SER_PORT &= ~_BV(SER); - digitalWrite(MOTORDATA, LOW); - - for (i=0; i<8; i++) { - //CLK_PORT &= ~_BV(CLK); - digitalWrite(MOTORCLK, LOW); - - if (latch_state & _BV(7-i)) { - //SER_PORT |= _BV(SER); - digitalWrite(MOTORDATA, HIGH); - } else { - //SER_PORT &= ~_BV(SER); - digitalWrite(MOTORDATA, LOW); - } - //CLK_PORT |= _BV(CLK); - digitalWrite(MOTORCLK, HIGH); - } - //LATCH_PORT |= _BV(LATCH); - digitalWrite(MOTORLATCH, HIGH); -} - -static UCMotorController MC; - -/****************************************** - MOTORS -******************************************/ -inline void initPWM1(uint8_t freq) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer2A on PB3 (Arduino pin #11) - TCCR2A |= _BV(COM2A1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2a - TCCR2B = freq & 0x7; - OCR2A = 0; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 11 is now PB5 (OC1A) - TCCR1A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc1a - TCCR1B = (freq & 0x7) | _BV(WGM12); - OCR1A = 0; -#elif defined(__PIC32MX__) - #if defined(PIC32_USE_PIN9_FOR_M1_PWM) - // Make sure that pin 11 is an input, since we have tied together 9 and 11 - pinMode(9, OUTPUT); - pinMode(11, INPUT); - if (!MC.TimerInitalized) - { // Set up Timer2 for 80MHz counting fro 0 to 256 - T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 - TMR2 = 0x0000; - PR2 = 0x0100; - MC.TimerInitalized = true; - } - // Setup OC4 (pin 9) in PWM mode, with Timer2 as timebase - OC4CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 - OC4RS = 0x0000; - OC4R = 0x0000; - #elif defined(PIC32_USE_PIN10_FOR_M1_PWM) - // Make sure that pin 11 is an input, since we have tied together 9 and 11 - pinMode(10, OUTPUT); - pinMode(11, INPUT); - if (!MC.TimerInitalized) - { // Set up Timer2 for 80MHz counting fro 0 to 256 - T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 - TMR2 = 0x0000; - PR2 = 0x0100; - MC.TimerInitalized = true; - } - // Setup OC5 (pin 10) in PWM mode, with Timer2 as timebase - OC5CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 - OC5RS = 0x0000; - OC5R = 0x0000; - #else - // If we are not using PWM for pin 11, then just do digital - digitalWrite(11, LOW); - #endif -#else - #error "This chip is not supported!" -#endif - #if !defined(PIC32_USE_PIN9_FOR_M1_PWM) && !defined(PIC32_USE_PIN10_FOR_M1_PWM) - pinMode(11, OUTPUT); - //Serial .println("OK"); - #endif -} - -inline void setPWM1(uint8_t s) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer2A on PB3 (Arduino pin #11) - OCR2A = s; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 11 is now PB5 (OC1A) - OCR1A = s; -#elif defined(__PIC32MX__) - #if defined(PIC32_USE_PIN9_FOR_M1_PWM) - // Set the OC4 (pin 9) PMW duty cycle from 0 to 255 - OC4RS = s; - #elif defined(PIC32_USE_PIN10_FOR_M1_PWM) - // Set the OC5 (pin 10) PMW duty cycle from 0 to 255 - OC5RS = s; - #else - // If we are not doing PWM output for M1, then just use on/off - if (s > 127) - { - digitalWrite(11, HIGH); - } - else - { - digitalWrite(11, LOW); - } - #endif -#else - #error "This chip is not supported!" -#endif -} - -inline void initPWM2(uint8_t freq) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer2B (pin 3) - TCCR2A |= _BV(COM2B1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2b - TCCR2B = freq & 0x7; - OCR2B = 0; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 3 is now PE5 (OC3C) - TCCR3A |= _BV(COM1C1) | _BV(WGM10); // fast PWM, turn on oc3c - TCCR3B = (freq & 0x7) | _BV(WGM12); - OCR3C = 0; -#elif defined(__PIC32MX__) - if (!MC.TimerInitalized) - { // Set up Timer2 for 80MHz counting fro 0 to 256 - T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 - TMR2 = 0x0000; - PR2 = 0x0100; - MC.TimerInitalized = true; - } - // Setup OC1 (pin3) in PWM mode, with Timer2 as timebase - OC1CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 - OC1RS = 0x0000; - OC1R = 0x0000; -#else - #error "This chip is not supported!" -#endif - - pinMode(3, OUTPUT); -} - -inline void setPWM2(uint8_t s) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer2A on PB3 (Arduino pin #11) - OCR2B = s; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 11 is now PB5 (OC1A) - OCR3C = s; -#elif defined(__PIC32MX__) - // Set the OC1 (pin3) PMW duty cycle from 0 to 255 - OC1RS = s; -#else - #error "This chip is not supported!" -#endif -} - -inline void initPWM3(uint8_t freq) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer0A / PD6 (pin 6) - TCCR0A |= _BV(COM0A1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on OC0A - //TCCR0B = freq & 0x7; - OCR0A = 0; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 6 is now PH3 (OC4A) - TCCR4A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc4a - TCCR4B = (freq & 0x7) | _BV(WGM12); - //TCCR4B = 1 | _BV(WGM12); - OCR4A = 0; -#elif defined(__PIC32MX__) - if (!MC.TimerInitalized) - { // Set up Timer2 for 80MHz counting fro 0 to 256 - T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 - TMR2 = 0x0000; - PR2 = 0x0100; - MC.TimerInitalized = true; - } - // Setup OC3 (pin 6) in PWM mode, with Timer2 as timebase - OC3CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 - OC3RS = 0x0000; - OC3R = 0x0000; -#else - #error "This chip is not supported!" -#endif - pinMode(6, OUTPUT); -} - -inline void setPWM3(uint8_t s) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer0A on PB3 (Arduino pin #6) - OCR0A = s; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 6 is now PH3 (OC4A) - OCR4A = s; -#elif defined(__PIC32MX__) - // Set the OC3 (pin 6) PMW duty cycle from 0 to 255 - OC3RS = s; -#else - #error "This chip is not supported!" -#endif -} - - - -inline void initPWM4(uint8_t freq) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer0B / PD5 (pin 5) - TCCR0A |= _BV(COM0B1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on oc0a - //TCCR0B = freq & 0x7; - OCR0B = 0; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 5 is now PE3 (OC3A) - TCCR3A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc3a - TCCR3B = (freq & 0x7) | _BV(WGM12); - //TCCR4B = 1 | _BV(WGM12); - OCR3A = 0; -#elif defined(__PIC32MX__) - if (!MC.TimerInitalized) - { // Set up Timer2 for 80MHz counting fro 0 to 256 - T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 - TMR2 = 0x0000; - PR2 = 0x0100; - MC.TimerInitalized = true; - } - // Setup OC2 (pin 5) in PWM mode, with Timer2 as timebase - OC2CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 - OC2RS = 0x0000; - OC2R = 0x0000; -#else - #error "This chip is not supported!" -#endif - pinMode(5, OUTPUT); -} - -inline void setPWM4(uint8_t s) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer0A on PB3 (Arduino pin #6) - OCR0B = s; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 6 is now PH3 (OC4A) - OCR3A = s; -#elif defined(__PIC32MX__) - // Set the OC2 (pin 5) PMW duty cycle from 0 to 255 - OC2RS = s; -#else - #error "This chip is not supported!" -#endif -} - -UC_DCMotor::UC_DCMotor(uint8_t num, uint8_t freq) { - motornum = num; - pwmfreq = freq; - MC.enable(); - - switch (num) { - case 1: - latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B); // set both motor pins to 0 - MC.latch_tx(); - initPWM1(freq); - break; - case 2: - latch_state &= ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // set both motor pins to 0 - MC.latch_tx(); - initPWM2(freq); - break; - case 3: - latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B); // set both motor pins to 0 - MC.latch_tx(); - initPWM3(freq); - break; - case 4: - latch_state &= ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // set both motor pins to 0 - MC.latch_tx(); - initPWM4(freq); - break; - } -} - -void UC_DCMotor::run(uint8_t cmd) { - uint8_t a, b; - switch (motornum) { - case 1: - a = MOTOR1_A; b = MOTOR1_B; break; - case 2: - a = MOTOR2_A; b = MOTOR2_B; break; - case 3: - a = MOTOR3_A; b = MOTOR3_B; break; - case 4: - a = MOTOR4_A; b = MOTOR4_B; break; - default: - return; - } - - switch (cmd) { - case FORWARD: - if(motornum ==3 || motornum ==4){ - latch_state |= _BV(a); - latch_state &= ~_BV(b); - }else{ - latch_state &= ~_BV(a); - latch_state |= _BV(b); - } - MC.latch_tx(); - break; - case BACKWARD: - if(motornum ==3 || motornum ==4){ - latch_state &= ~_BV(a); - latch_state |= _BV(b); - }else{ - latch_state |= _BV(a); - latch_state &= ~_BV(b); - } - MC.latch_tx(); - break; - case LEFT: - if(motornum ==3){ - a = MOTOR3_A; b = MOTOR3_B; //backward - latch_state &= ~_BV(a); - latch_state |= _BV(b); - MC.latch_tx();} - else if(motornum ==4){ - a = MOTOR4_A; b = MOTOR4_B; - latch_state |= _BV(a); - latch_state &= ~_BV(b); - MC.latch_tx(); - }else if(motornum ==1){ - a = MOTOR1_A; b = MOTOR1_B; - latch_state |= _BV(a); - latch_state &= ~_BV(b); - MC.latch_tx(); - }else if(motornum ==2){ - a = MOTOR2_A; b = MOTOR2_B; - latch_state &= ~_BV(a); - latch_state |= _BV(b); - MC.latch_tx(); - } - break; - case RIGHT: - if(motornum ==3){ - a = MOTOR3_A; b = MOTOR3_B; //backward - latch_state |= _BV(a); - latch_state &= ~_BV(b); - MC.latch_tx();} - else if(motornum ==4){ - a = MOTOR4_A; b = MOTOR4_B; - latch_state &= ~_BV(a); - latch_state |= _BV(b); - MC.latch_tx();}else if(motornum ==1){ - a = MOTOR1_A; b = MOTOR1_B; - latch_state &= ~_BV(a); - latch_state |= _BV(b); - MC.latch_tx();}else if(motornum ==2){ - a = MOTOR2_A; b = MOTOR2_B; - latch_state |= _BV(a); - latch_state &= ~_BV(b); - MC.latch_tx(); - } - break; - case STOP: - latch_state &= ~_BV(a); // A and B both low - latch_state &= ~_BV(b); - MC.latch_tx(); - break; - } -} - -void UC_DCMotor::setSpeed(uint8_t speed) { - switch (motornum) { - case 1: - setPWM1(speed); break; - case 2: - setPWM2(speed); break; - case 3: - setPWM3(speed); break; - case 4: - setPWM4(speed); break; - } -} - -/****************************************** - STEPPERS -******************************************/ - -UC_Stepper::UC_Stepper(uint16_t steps, uint8_t num) { - MC.enable(); - - revsteps = steps; - steppernum = num; - currentstep = 0; - - if (steppernum == 1) { - latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) & - ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0 - MC.latch_tx(); - - // enable both H bridges - pinMode(11, OUTPUT); - pinMode(3, OUTPUT); - digitalWrite(11, HIGH); - digitalWrite(3, HIGH); - - // use PWM for microstepping support - initPWM1(STEPPER1_PWM_RATE); - initPWM2(STEPPER1_PWM_RATE); - setPWM1(255); - setPWM2(255); - - } else if (steppernum == 2) { - latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) & - ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0 - MC.latch_tx(); - - // enable both H bridges - pinMode(5, OUTPUT); - pinMode(6, OUTPUT); - digitalWrite(5, HIGH); - digitalWrite(6, HIGH); - - // use PWM for microstepping support - // use PWM for microstepping support - initPWM3(STEPPER2_PWM_RATE); - initPWM4(STEPPER2_PWM_RATE); - setPWM3(255); - setPWM4(255); - } -} - -void UC_Stepper::setSpeed(uint16_t rpm) { - usperstep = 60000000 / ((uint32_t)revsteps * (uint32_t)rpm); - steppingcounter = 0; -} - -void UC_Stepper::release(void) { - if (steppernum == 1) { - latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) & - ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0 - MC.latch_tx(); - } else if (steppernum == 2) { - latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) & - ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0 - MC.latch_tx(); - } -} - -void UC_Stepper::step(uint16_t steps, uint8_t dir, uint8_t style) { - uint32_t uspers = usperstep; - uint8_t ret = 0; - - if (style == INTERLEAVE) { - uspers /= 2; - } - else if (style == MICROSTEP) { - uspers /= MICROSTEPS; - steps *= MICROSTEPS; -#ifdef MOTORDEBUG - Serial.print("steps = "); Serial.println(steps, DEC); -#endif - } - - while (steps--) { - ret = onestep(dir, style); - delay(uspers/1000); // in ms - steppingcounter += (uspers % 1000); - if (steppingcounter >= 1000) { - delay(1); - steppingcounter -= 1000; - } - } - if (style == MICROSTEP) { - while ((ret != 0) && (ret != MICROSTEPS)) { - ret = onestep(dir, style); - delay(uspers/1000); // in ms - steppingcounter += (uspers % 1000); - if (steppingcounter >= 1000) { - delay(1); - steppingcounter -= 1000; - } - } - } -} - -uint8_t UC_Stepper::onestep(uint8_t dir, uint8_t style) { - uint8_t a, b, c, d; - uint8_t ocrb, ocra; - - ocra = ocrb = 255; - - if (steppernum == 1) { - a = _BV(MOTOR1_A); - b = _BV(MOTOR2_A); - c = _BV(MOTOR1_B); - d = _BV(MOTOR2_B); - } else if (steppernum == 2) { - a = _BV(MOTOR3_A); - b = _BV(MOTOR4_A); - c = _BV(MOTOR3_B); - d = _BV(MOTOR4_B); - } else { - return 0; - } - - // next determine what sort of stepping procedure we're up to - if (style == SINGLE) { - if ((currentstep/(MICROSTEPS/2)) % 2) { // we're at an odd step, weird - if (dir == FORWARD) { - currentstep += MICROSTEPS/2; - } - else { - currentstep -= MICROSTEPS/2; - } - } else { // go to the next even step - if (dir == FORWARD) { - currentstep += MICROSTEPS; - } - else { - currentstep -= MICROSTEPS; - } - } - } else if (style == DOUBLE) { - if (! (currentstep/(MICROSTEPS/2) % 2)) { // we're at an even step, weird - if (dir == FORWARD) { - currentstep += MICROSTEPS/2; - } else { - currentstep -= MICROSTEPS/2; - } - } else { // go to the next odd step - if (dir == FORWARD) { - currentstep += MICROSTEPS; - } else { - currentstep -= MICROSTEPS; - } - } - } else if (style == INTERLEAVE) { - if (dir == FORWARD) { - currentstep += MICROSTEPS/2; - } else { - currentstep -= MICROSTEPS/2; - } - } - - if (style == MICROSTEP) { - if (dir == FORWARD) { - currentstep++; - } else { - // BACKWARDS - currentstep--; - } - - currentstep += MICROSTEPS*4; - currentstep %= MICROSTEPS*4; - - ocra = ocrb = 0; - if ( (currentstep >= 0) && (currentstep < MICROSTEPS)) { - ocra = microstepcurve[MICROSTEPS - currentstep]; - ocrb = microstepcurve[currentstep]; - } else if ( (currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) { - ocra = microstepcurve[currentstep - MICROSTEPS]; - ocrb = microstepcurve[MICROSTEPS*2 - currentstep]; - } else if ( (currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) { - ocra = microstepcurve[MICROSTEPS*3 - currentstep]; - ocrb = microstepcurve[currentstep - MICROSTEPS*2]; - } else if ( (currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) { - ocra = microstepcurve[currentstep - MICROSTEPS*3]; - ocrb = microstepcurve[MICROSTEPS*4 - currentstep]; - } - } - - currentstep += MICROSTEPS*4; - currentstep %= MICROSTEPS*4; - -#ifdef MOTORDEBUG - Serial.print("current step: "); Serial.println(currentstep, DEC); - Serial.print(" pwmA = "); Serial.print(ocra, DEC); - Serial.print(" pwmB = "); Serial.println(ocrb, DEC); -#endif - - if (steppernum == 1) { - setPWM1(ocra); - setPWM2(ocrb); - } else if (steppernum == 2) { - setPWM3(ocra); - setPWM4(ocrb); - } - - - // release all - latch_state &= ~a & ~b & ~c & ~d; // all motor pins to 0 - - //Serial.println(step, DEC); - if (style == MICROSTEP) { - if ((currentstep >= 0) && (currentstep < MICROSTEPS)) - latch_state |= a | b; - if ((currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) - latch_state |= b | c; - if ((currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) - latch_state |= c | d; - if ((currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) - latch_state |= d | a; - } else { - switch (currentstep/(MICROSTEPS/2)) { - case 0: - latch_state |= a; // energize coil 1 only - break; - case 1: - latch_state |= a | b; // energize coil 1+2 - break; - case 2: - latch_state |= b; // energize coil 2 only - break; - case 3: - latch_state |= b | c; // energize coil 2+3 - break; - case 4: - latch_state |= c; // energize coil 3 only - break; - case 5: - latch_state |= c | d; // energize coil 3+4 - break; - case 6: - latch_state |= d; // energize coil 4 only - break; - case 7: - latch_state |= d | a; // energize coil 1+4 - break; - } - } - - - MC.latch_tx(); - return currentstep; -} - diff --git a/vehicle_interface/main/UCMotor.h b/vehicle_interface/main/UCMotor.h deleted file mode 100644 index 1e2f1e05..00000000 --- a/vehicle_interface/main/UCMotor.h +++ /dev/null @@ -1,195 +0,0 @@ -/** - * @file UCMotor.h - * @author - * @brief This was fetched from https://github.com/UCTRONICS/Smart-Robot-Car-Arduino/tree/master/UCTRONICS_Smart_Robot_Car - * - * @note No edits have been made to to this file aside from the brief - * - */ -// UCTRONICS Motor shield library -// this code is public domain, enjoy! - -/* - * Usage Notes: - * For PIC32, all features work properly with the following two exceptions: - * - * 1) Because the PIC32 only has 5 PWM outputs, and the UCMotor shield needs 6 - * to completely operate (four for motor outputs and two for RC servos), the - * M1 motor output will not have PWM ability when used with a PIC32 board. - * However, there is a very simple workaround. If you need to drive a stepper - * or DC motor with PWM on motor output M1, you can use the PWM output on pin - * 9 or pin 10 (normally use for RC servo outputs on Arduino, not needed for - * RC servo outputs on PIC32) to drive the PWM input for M1 by simply putting - * a jumber from pin 9 to pin 11 or pin 10 to pin 11. Then uncomment one of the - * two #defines below to activate the PWM on either pin 9 or pin 10. You will - * then have a fully functional microstepping for 2 stepper motors, or four - * DC motor outputs with PWM. - * - * 2) There is a conflict between RC Servo outputs on pins 9 and pins 10 and - * the operation of DC motors and stepper motors as of 9/2012. This issue - * will get fixed in future MPIDE releases, but at the present time it means - * that the Motor Party example will NOT work properly. Any time you attach - * an RC servo to pins 9 or pins 10, ALL PWM outputs on the whole board will - * stop working. Thus no steppers or DC motors. - * - */ -// 09/15/2012 Modified for use with chipKIT boards - - -#ifndef _UCMotor_h_ -#define _UCMotor_h_ - -#include -#if defined(__AVR__) - #include - - //#define MOTORDEBUG 1 - - #define MICROSTEPS 16 // 8 or 16 - - #define MOTOR12_64KHZ _BV(CS20) // no prescale - #define MOTOR12_8KHZ _BV(CS21) // divide by 8 - #define MOTOR12_2KHZ _BV(CS21) | _BV(CS20) // divide by 32 - #define MOTOR12_1KHZ _BV(CS22) // divide by 64 - - #define MOTOR34_64KHZ _BV(CS00) // no prescale - #define MOTOR34_8KHZ _BV(CS01) // divide by 8 - #define MOTOR34_1KHZ _BV(CS01) | _BV(CS00) // divide by 64 - - #define DC_MOTOR_PWM_RATE MOTOR34_8KHZ // PWM rate for DC motors - #define STEPPER1_PWM_RATE MOTOR12_64KHZ // PWM rate for stepper 1 - #define STEPPER2_PWM_RATE MOTOR34_64KHZ // PWM rate for stepper 2 - -#elif defined(__PIC32MX__) - //#define MOTORDEBUG 1 - - // Uncomment the one of following lines if you have put a jumper from - // either pin 9 to pin 11 or pin 10 to pin 11 on your Motor Shield. - // Either will enable PWM for M1 - //#define PIC32_USE_PIN9_FOR_M1_PWM - //#define PIC32_USE_PIN10_FOR_M1_PWM - - #define MICROSTEPS 16 // 8 or 16 - - // For PIC32 Timers, define prescale settings by PWM frequency - #define MOTOR12_312KHZ 0 // 1:1, actual frequency 312KHz - #define MOTOR12_156KHZ 1 // 1:2, actual frequency 156KHz - #define MOTOR12_64KHZ 2 // 1:4, actual frequency 78KHz - #define MOTOR12_39KHZ 3 // 1:8, acutal frequency 39KHz - #define MOTOR12_19KHZ 4 // 1:16, actual frequency 19KHz - #define MOTOR12_8KHZ 5 // 1:32, actual frequency 9.7KHz - #define MOTOR12_4_8KHZ 6 // 1:64, actual frequency 4.8KHz - #define MOTOR12_2KHZ 7 // 1:256, actual frequency 1.2KHz - #define MOTOR12_1KHZ 7 // 1:256, actual frequency 1.2KHz - - #define MOTOR34_312KHZ 0 // 1:1, actual frequency 312KHz - #define MOTOR34_156KHZ 1 // 1:2, actual frequency 156KHz - #define MOTOR34_64KHZ 2 // 1:4, actual frequency 78KHz - #define MOTOR34_39KHZ 3 // 1:8, acutal frequency 39KHz - #define MOTOR34_19KHZ 4 // 1:16, actual frequency 19KHz - #define MOTOR34_8KHZ 5 // 1:32, actual frequency 9.7KHz - #define MOTOR34_4_8KHZ 6 // 1:64, actual frequency 4.8KHz - #define MOTOR34_2KHZ 7 // 1:256, actual frequency 1.2KHz - #define MOTOR34_1KHZ 7 // 1:256, actual frequency 1.2KHz - - // PWM rate for DC motors. - #define DC_MOTOR_PWM_RATE MOTOR34_39KHZ - // Note: for PIC32, both of these must be set to the same value - // since there's only one timebase for all 4 PWM outputs - #define STEPPER1_PWM_RATE MOTOR12_39KHZ - #define STEPPER2_PWM_RATE MOTOR34_39KHZ - -#endif - -// Bit positions in the 74HCT595 shift register output -#define MOTOR1_A 2 -#define MOTOR1_B 3 -#define MOTOR2_A 1 -#define MOTOR2_B 4 -#define MOTOR4_A 0 -#define MOTOR4_B 6 -#define MOTOR3_A 5 -#define MOTOR3_B 7 - -// Constants that the user passes in to the motor calls -#define FORWARD 1 -#define BACKWARD 2 -#define LEFT 3 -#define RIGHT 4 -#define STOP 5 - -#define FORWARD2 2 -#define BACKWARD2 1 - -#define BRAKE 3 -#define RELEASE 4 - -// Constants that the user passes in to the stepper calls -#define SINGLE 1 -#define DOUBLE 2 -#define INTERLEAVE 3 -#define MICROSTEP 4 - -/* -#define LATCH 4 -#define LATCH_DDR DDRB -#define LATCH_PORT PORTB - -#define CLK_PORT PORTD -#define CLK_DDR DDRD -#define CLK 4 - -#define ENABLE_PORT PORTD -#define ENABLE_DDR DDRD -#define ENABLE 7 - -#define SER 0 -#define SER_DDR DDRB -#define SER_PORT PORTB -*/ - -// Arduino pin names for interface to 74HCT595 latch -#define MOTORLATCH 12 -#define MOTORCLK 4 -#define MOTORENABLE 7 -#define MOTORDATA 8 - -class UCMotorController -{ - public: - UCMotorController(void); - void enable(void); - friend class UC_DCMotor; - void latch_tx(void); - uint8_t TimerInitalized; -}; - -class UC_DCMotor -{ - public: - UC_DCMotor(uint8_t motornum, uint8_t freq = DC_MOTOR_PWM_RATE); - void run(uint8_t); - void setSpeed(uint8_t); - - private: - uint8_t motornum, pwmfreq; -}; - -class UC_Stepper { - public: - UC_Stepper(uint16_t, uint8_t); - void step(uint16_t steps, uint8_t dir, uint8_t style = SINGLE); - void setSpeed(uint16_t); - uint8_t onestep(uint8_t dir, uint8_t style); - void release(void); - uint16_t revsteps; // # steps per revolution - uint8_t steppernum; - uint32_t usperstep, steppingcounter; - private: - uint8_t currentstep; - -}; - -uint8_t getlatchstate(void); - -#endif diff --git a/vehicle_interface/main/main.ino b/vehicle_interface/main/main.ino index fad7faf7..611c4e33 100644 --- a/vehicle_interface/main/main.ino +++ b/vehicle_interface/main/main.ino @@ -3,6 +3,7 @@ #include "UCMotor.h" #include #include +#include //You will need to install this library volatile Array commands = all_brake_command; @@ -26,6 +27,9 @@ void setup() pinMode(RC_SWA_CHANNEL_PIN, INPUT); pinMode(RC_SWB_CHANNEL_PIN, INPUT); + //Initialize I2C protocol for ultrasonic sensor + i2c_init(); + // Indicate setup done Serial.println("Setup complete"); } @@ -60,11 +64,80 @@ void sendCommandMini(UC_DCMotor * uc_motor, wheel_motor_command_t command) { void loop() { + int distance = read_ultrasonic_sensor(); + if (distance > MIN_DISTANCE){ // Update wheel commands commands = fetch_rc_commands(); + } + else{ + commands = all_brake_command; + } // Send commands to wheels for (int i = wheel_indices::Start + 1; i < wheel_indices::End; i++ ) { sendCommandMini(minis[i], commands[i]); } } + +////////////////////////////////////////////////////////// +// Read the sensor at the default address // +////////////////////////////////////////////////////////// +int read_ultrasonic_sensor(){ + boolean error = 0; //Create a bit to check for catch errors as needed. + int distance; + + //Take a range reading at the default address of 224 + error = start_sensor(224); //Start the sensor and collect any error codes. + if (!error){ //If you had an error starting the sensor there is little point in reading it as you will get old data. + delay(100); + distance = read_sensor(224); //reading the sensor will return an integer value -- if this value is 0 there was an error + //Serial.print("Distance:");Serial.println(range);Serial.print("cm"); + Serial.println((String)"Distance:"+distance+" cm"); + return distance; + } +} + +/////////////////////////////////////////////////// +// Function: Start a range reading on the sensor // +/////////////////////////////////////////////////// +//Uses the I2C library to start a sensor at the given address +//Collects and reports an error bit where: 1 = there was an error or 0 = there was no error. +//INPUTS: byte bit8address = the address of the sensor that we want to command a range reading +//OUPUTS: bit errorlevel = reports if the function was successful in taking a range reading: 1 = the function +// had an error, 0 = the function was successful +boolean start_sensor(byte bit8address){ + boolean errorlevel = 0; + bit8address = bit8address & B11111110; //Do a bitwise 'and' operation to force the last bit to be zero -- we are writing to the address. + errorlevel = !i2c_start(bit8address) | errorlevel; //Run i2c_start(address) while doing so, collect any errors where 1 = there was an error. + errorlevel = !i2c_write(81) | errorlevel; //Send the 'take range reading' command. (notice how the library has error = 0 so I had to use "!" (not) to invert the error) + i2c_stop(); + return errorlevel; +} + + + +/////////////////////////////////////////////////////////////////////// +// Function: Read the range from the sensor at the specified address // +/////////////////////////////////////////////////////////////////////// +//Uses the I2C library to read a sensor at the given address +//Collects errors and reports an invalid range of "0" if there was a problem. +//INPUTS: byte bit8address = the address of the sensor to read from +//OUPUTS: int range = the distance in cm that the sensor reported; if "0" there was a communication error +int read_sensor(byte bit8address){ + boolean errorlevel = 0; + int range = 0; + byte range_highbyte = 0; + byte range_lowbyte = 0; + bit8address = bit8address | B00000001; //Do a bitwise 'or' operation to force the last bit to be 'one' -- we are reading from the address. + errorlevel = !i2c_start(bit8address) | errorlevel; + range_highbyte = i2c_read(0); //Read a byte and send an ACK (acknowledge) + range_lowbyte = i2c_read(1); //Read a byte and send a NACK to terminate the transmission + i2c_stop(); + range = (range_highbyte * 256) + range_lowbyte; //compile the range integer from the two bytes received. + if(errorlevel){ + return 0; + } + else{ + return range; + } +} diff --git a/vehicle_interface/main/pinout.h b/vehicle_interface/main/pinout.h index f507f43a..15563540 100644 --- a/vehicle_interface/main/pinout.h +++ b/vehicle_interface/main/pinout.h @@ -4,6 +4,11 @@ // Pins defined by the UCMotor header #include "UCMotor.h" +//Pin definitions for the ultrasonic sensor(MB1202) +#define SCL_PIN 5 //Default SDA is Pin5 PORTC for the UNO -- you can set this to any tristate pin +#define SCL_PORT PORTC +#define SDA_PIN 4 //Default SCL is Pin4 PORTC for the UNO -- you can set this to any tristate pin +#define SDA_PORT PORTC // Pin definitions for the RC receiver #ifdef __AVR_ATmega328P__ @@ -22,4 +27,4 @@ #error "No RC pin configurations available" #endif -#endif // !PINOUT_H +#endif // !PINOUT_H diff --git a/vehicle_interface/main/rc_commands.cpp b/vehicle_interface/main/rc_commands.cpp index 61af19f2..7910f36c 100644 --- a/vehicle_interface/main/rc_commands.cpp +++ b/vehicle_interface/main/rc_commands.cpp @@ -1,67 +1,67 @@ -#include "rc_commands.h" -#include "util.h" - - -/** - * @brief fetches RC commands and returns motor commands - * - * @return returns array of motor commands - */ -Array fetch_rc_commands() -{ - // Get commands - uint16_t sw_a = pulseIn(RC_SWA_CHANNEL_PIN, HIGH); - uint16_t sw_b = pulseIn(RC_SWB_CHANNEL_PIN, HIGH); - uint16_t rc_right = pulseIn(RC_RIGHT_CHANNEL_PIN, HIGH); - uint16_t rc_left = pulseIn(RC_LEFT_CHANNEL_PIN, HIGH); - - // define temp variables - float right_duty = 0.0; - bool right_dir = DIR_FW; // default direction - float left_duty = 0.0; - bool left_dir = DIR_FW; // default direction - bool autonomy_mode = false; - - /* Switches */ - // Check if stop asserted - if (sw_a < RC_SWX_HIGH_MAX && sw_a > RC_SWX_HIGH_MIN) { - return all_brake_command; // fix to also return mode - } - // Check if mode changed - if (sw_b < RC_SWX_HIGH_MAX && sw_b > RC_SWX_HIGH_MIN) { - autonomy_mode = true; - } - - /* Right side longitudinal wheel set */ - // Forward - if (rc_right < RC_RIGHT_SET_FW_MAX && rc_right > RC_RIGHT_SET_FW_MIN) { - // map the duty from 0 to 1 given the min and max threshold values - right_duty = linMapToFloat(rc_right, RC_RIGHT_SET_FW_MIN, RC_RIGHT_SET_FW_MAX, 0, 1); - } - // Backward - else if (rc_right < RC_RIGHT_SET_BW_MAX && rc_right > RC_RIGHT_SET_BW_MIN) - { - right_duty = 1 - linMapToFloat(rc_right, RC_RIGHT_SET_BW_MIN, RC_RIGHT_SET_BW_MAX, 0, 1); - right_dir = DIR_BW; - } - /* Left side longitudinal wheel set */ - // Forward - if (rc_left < RC_LEFT_SET_FW_MAX && rc_left > RC_LEFT_SET_FW_MIN) { - left_duty = linMapToFloat(rc_left, RC_LEFT_SET_FW_MIN, RC_LEFT_SET_FW_MAX, 0, 1); - } - // Backward - else if (rc_left < RC_LEFT_SET_BW_MAX && rc_left > RC_LEFT_SET_BW_MIN) - { - left_duty = 1 - linMapToFloat(rc_left, RC_LEFT_SET_BW_MIN, RC_LEFT_SET_BW_MAX, 0, 1); - left_dir = DIR_BW; - } - - /* Define and fill wheel motor commands */ - Array wheel_motor_commands = {{ - {left_duty, RELEASE_BRAKE, left_dir}, - {right_duty, RELEASE_BRAKE, right_dir}, - }}; - - /* Return reference to the command set */ - return wheel_motor_commands; -} +#include "rc_commands.h" +#include "util.h" + + +/** + * @brief fetches RC commands and returns motor commands + * + * @return returns array of motor commands + */ +Array fetch_rc_commands() +{ + // Get commands + uint16_t sw_a = pulseIn(RC_SWA_CHANNEL_PIN, HIGH); + uint16_t sw_b = pulseIn(RC_SWB_CHANNEL_PIN, HIGH); + uint16_t rc_right = pulseIn(RC_RIGHT_CHANNEL_PIN, HIGH); + uint16_t rc_left = pulseIn(RC_LEFT_CHANNEL_PIN, HIGH); + + // define temp variables + float right_duty = 0.0; + bool right_dir = DIR_FW; // default direction + float left_duty = 0.0; + bool left_dir = DIR_FW; // default direction + bool autonomy_mode = false; + + /* Switches */ + // Check if stop asserted + if (sw_a < RC_SWX_HIGH_MAX && sw_a > RC_SWX_HIGH_MIN) { + return all_brake_command; // fix to also return mode + } + // Check if mode changed + if (sw_b < RC_SWX_HIGH_MAX && sw_b > RC_SWX_HIGH_MIN) { + autonomy_mode = true; + } + + /* Right side longitudinal wheel set */ + // Forward + if (rc_right < RC_RIGHT_SET_FW_MAX && rc_right > RC_RIGHT_SET_FW_MIN) { + // map the duty from 0 to 1 given the min and max threshold values + right_duty = linMapToFloat(rc_right, RC_RIGHT_SET_FW_MIN, RC_RIGHT_SET_FW_MAX, 0, 1); + } + // Backward + else if (rc_right < RC_RIGHT_SET_BW_MAX && rc_right > RC_RIGHT_SET_BW_MIN) + { + right_duty = 1 - linMapToFloat(rc_right, RC_RIGHT_SET_BW_MIN, RC_RIGHT_SET_BW_MAX, 0, 1); + right_dir = DIR_BW; + } + /* Left side longitudinal wheel set */ + // Forward + if (rc_left < RC_LEFT_SET_FW_MAX && rc_left > RC_LEFT_SET_FW_MIN) { + left_duty = linMapToFloat(rc_left, RC_LEFT_SET_FW_MIN, RC_LEFT_SET_FW_MAX, 0, 1); + } + // Backward + else if (rc_left < RC_LEFT_SET_BW_MAX && rc_left > RC_LEFT_SET_BW_MIN) + { + left_duty = 1 - linMapToFloat(rc_left, RC_LEFT_SET_BW_MIN, RC_LEFT_SET_BW_MAX, 0, 1); + left_dir = DIR_BW; + } + + /* Define and fill wheel motor commands */ + Array wheel_motor_commands = {{ + {left_duty, RELEASE_BRAKE, left_dir}, + {right_duty, RELEASE_BRAKE, right_dir}, + }}; + + /* Return reference to the command set */ + return wheel_motor_commands; +} diff --git a/vehicle_interface/main/rc_commands.h b/vehicle_interface/main/rc_commands.h index c47125c0..2076b192 100644 --- a/vehicle_interface/main/rc_commands.h +++ b/vehicle_interface/main/rc_commands.h @@ -22,7 +22,9 @@ #define RC_SWX_HIGH_MAX 2000//1981 #define RC_SWX_HIGH_MIN 1900//1974 +/* Define the minimum distance that the ultrasonic sensor can detect*/ +#define MIN_DISTANCE 25 /* Function prototypes for rc commands */ Array fetch_rc_commands(void); -#endif // !RC_COMMANDS_H \ No newline at end of file +#endif // !RC_COMMANDS_H diff --git a/vehicle_interface/main/util.cpp b/vehicle_interface/main/util.cpp index 17dc0b7a..04af3169 100644 --- a/vehicle_interface/main/util.cpp +++ b/vehicle_interface/main/util.cpp @@ -1,14 +1,14 @@ -/** - * @brief Returns the input linearly mapped to [0, 1] based on the provided I/O minima and maxima. - * - * @param x input value - * @param in_min input minimum - * @param in_max input maximum - * @param out_min output minimum - * @param out_max output maximum - * @return float - */ -float linMapToFloat(float x, float in_min, float in_max, float out_min, float out_max) -{ - return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; -} +/** + * @brief Returns the input linearly mapped to [0, 1] based on the provided I/O minima and maxima. + * + * @param x input value + * @param in_min input minimum + * @param in_max input maximum + * @param out_min output minimum + * @param out_max output maximum + * @return float + */ +float linMapToFloat(float x, float in_min, float in_max, float out_min, float out_max) +{ + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} diff --git a/vehicle_interface/main/util.h b/vehicle_interface/main/util.h index 40a279e9..b62bc666 100644 --- a/vehicle_interface/main/util.h +++ b/vehicle_interface/main/util.h @@ -1,53 +1,53 @@ -#ifndef UTIL_H -#define UTIL_H - -#include - -#define LN_SCALE_FACTOR -3.0 -#define MAX_DUTY 0.95 - -/** - * @brief Returns the input linearly mapped to [0, 1] based on the provided I/O minima and maxima. - * - * @param x input value - * @param in_min input minimum - * @param in_max input maximum - * @param out_min output minimum - * @param out_max output maximum - * @return float - */ -float linMapToFloat(float x, float in_min, float in_max, float out_min, float out_max); - -template -struct Array { - // Storage - T data[N]; - - static size_t length() { return N; } - using type = T; - - // Item access - T &operator[](size_t index) { return data[index]; } - const T &operator[](size_t index) const { return data[index]; } - - // Iterators - T *begin() { return &data[0]; } - const T *begin() const { return &data[0]; } - T *end() { return &data[N]; } - const T *end() const { return &data[N]; } - - // Comparisons - bool operator==(const Array &rhs) const { - if (this == &rhs) - return true; - for (size_t i = 0; i < N; i++) - if ((*this)[i] != rhs[i]) - return false; - return true; - } - bool operator!=(const Array &rhs) const { - return !(*this == rhs); - } -}; - -#endif // UTIL_H +#ifndef UTIL_H +#define UTIL_H + +#include + +#define LN_SCALE_FACTOR -3.0 +#define MAX_DUTY 0.95 + +/** + * @brief Returns the input linearly mapped to [0, 1] based on the provided I/O minima and maxima. + * + * @param x input value + * @param in_min input minimum + * @param in_max input maximum + * @param out_min output minimum + * @param out_max output maximum + * @return float + */ +float linMapToFloat(float x, float in_min, float in_max, float out_min, float out_max); + +template +struct Array { + // Storage + T data[N]; + + static size_t length() { return N; } + using type = T; + + // Item access + T &operator[](size_t index) { return data[index]; } + const T &operator[](size_t index) const { return data[index]; } + + // Iterators + T *begin() { return &data[0]; } + const T *begin() const { return &data[0]; } + T *end() { return &data[N]; } + const T *end() const { return &data[N]; } + + // Comparisons + bool operator==(const Array &rhs) const { + if (this == &rhs) + return true; + for (size_t i = 0; i < N; i++) + if ((*this)[i] != rhs[i]) + return false; + return true; + } + bool operator!=(const Array &rhs) const { + return !(*this == rhs); + } +}; + +#endif // UTIL_H diff --git a/vehicle_interface/main/wheel.h b/vehicle_interface/main/wheel.h index dce2afb0..e7db122e 100644 --- a/vehicle_interface/main/wheel.h +++ b/vehicle_interface/main/wheel.h @@ -9,7 +9,7 @@ /* Define Important Values */ #if defined(__AVR_ATmega328P__) #define TOP 0XFF - #elif (defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + #elif (defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)) #define TOP 0x03FF // Top value of the registers (configuration done in MAIN) #endif @@ -48,4 +48,4 @@ const Array all_brake_command = {{ {0.0, ENGAGE_BRAKE, FORWARD} }}; -#endif // !Wheel_h +#endif // !Wheel_h