Branch data     Line data    Source code

       1                 :             : #include "carMove.hpp"
       2                 :             : 
       3                 :           0 : void CarMove::setPWM(int device_handle, int channel, int on_value, int off_value) {
       4                 :           0 :     int reg_base = 0x06 + (channel * 4);
       5                 :           0 :     i2cWriteByteData(device_handle, reg_base, on_value & 0xFF);
       6                 :           0 :     i2cWriteByteData(device_handle, reg_base + 1, on_value >> 8);
       7                 :           0 :     i2cWriteByteData(device_handle, reg_base + 2, off_value & 0xFF);
       8                 :           0 :     i2cWriteByteData(device_handle, reg_base + 3, off_value >> 8);
       9                 :           0 : }
      10                 :             : 
      11                 :           0 : void CarMove::setServoAngle(int angle) {
      12                 :           0 :     angle = std::max(-MAX_ANGLE, std::min(45, angle));
      13                 :           0 :     int pwm = 0;
      14                 :           0 :     if (angle < 0) {
      15                 :           0 :         pwm = SERVO_CENTER_PWM +
      16                 :           0 :               (angle / static_cast<float>(MAX_ANGLE)) * (SERVO_CENTER_PWM - SERVO_LEFT_PWM);
      17                 :           0 :     } else if (angle > 0) {
      18                 :           0 :         pwm = SERVO_CENTER_PWM +
      19                 :           0 :               (angle / static_cast<float>(MAX_ANGLE)) * (SERVO_RIGHT_PWM - SERVO_CENTER_PWM);
      20                 :             :     } else {
      21                 :           0 :         pwm = SERVO_CENTER_PWM;
      22                 :             :     }
      23                 :           0 :     setPWM(servo_handle, STEERING_CHANNEL, 0, pwm);
      24                 :           0 : }
      25                 :             : 
      26                 :           0 : void CarMove::setMotorSpeed(int speed) {
      27                 :           0 :     speed = std::max(-100, std::min(100, speed));
      28                 :           0 :     int pwm_value = static_cast<int>(std::abs(speed) / 100.0 * 4095);
      29                 :             : 
      30                 :           0 :     if (speed > 0) {                            // Forward
      31                 :           0 :         setPWM(motor_handle, 1, pwm_value, 0);  // IN1
      32                 :           0 :         setPWM(motor_handle, 2, 0, pwm_value);  // IN2
      33                 :           0 :         setPWM(motor_handle, 0, 0, pwm_value);  // ENA
      34                 :             : 
      35                 :           0 :         setPWM(motor_handle, 5, 0, pwm_value);  // IN3
      36                 :           0 :         setPWM(motor_handle, 6, pwm_value, 0);  // IN4
      37                 :           0 :         setPWM(motor_handle, 7, 0, pwm_value);  // ENB
      38                 :           0 :     } else if (speed < 0) {                     // Backward
      39                 :           0 :         setPWM(motor_handle, 0, 0, pwm_value);  // ENA
      40                 :           0 :         setPWM(motor_handle, 1, 0, pwm_value);  // IN1
      41                 :           0 :         setPWM(motor_handle, 2, pwm_value, 0);  // IN2
      42                 :             : 
      43                 :           0 :         setPWM(motor_handle, 5, pwm_value, 0);  // IN3
      44                 :           0 :         setPWM(motor_handle, 6, 0, pwm_value);  // IN4
      45                 :           0 :         setPWM(motor_handle, 7, 0, pwm_value);  // ENB
      46                 :             : 
      47                 :             :     } else {  // Stop
      48                 :           0 :         for (int channel = 0; channel < 8; ++channel) {
      49                 :           0 :             setPWM(motor_handle, channel, 0, 0);
      50                 :             :         }
      51                 :             :     }
      52                 :           0 : }
      53                 :             : 
      54                 :           0 : CarMove::CarMove() {
      55                 :           0 :     if (gpioInitialise() < 0) {
      56                 :           0 :         throw std::runtime_error("Failed to initialize pigpio library");
      57                 :             :     }
      58                 :             : 
      59                 :           0 :     servo_handle = i2cOpen(1, SERVO_ADDR, 0);
      60                 :           0 :     motor_handle = i2cOpen(1, MOTOR_ADDR, 0);
      61                 :             : 
      62                 :           0 :     if (servo_handle < 0 || motor_handle < 0) {
      63                 :           0 :         gpioTerminate();
      64                 :           0 :         throw std::runtime_error("Failed to initialize I2C devices");
      65                 :             :     }
      66                 :             : 
      67                 :             :     // Initialize servo
      68                 :           0 :     i2cWriteByteData(servo_handle, 0x00, 0x10);
      69                 :           0 :     usleep(100000);
      70                 :           0 :     i2cWriteByteData(servo_handle, 0xFE, 0x79);  // Set ~50Hz
      71                 :           0 :     i2cWriteByteData(servo_handle, 0x00, 0x20);
      72                 :             : 
      73                 :             :     // Initialize motor
      74                 :           0 :     i2cWriteByteData(motor_handle, 0x00, 0x20);
      75                 :           0 :     int prescale = std::floor(25000000.0 / 4096.0 / 60.0 - 1);
      76                 :           0 :     int oldmode = i2cReadByteData(motor_handle, 0x00);
      77                 :           0 :     int newmode = (oldmode & 0x7F) | 0x10;
      78                 :           0 :     i2cWriteByteData(motor_handle, 0x00, newmode);
      79                 :           0 :     i2cWriteByteData(motor_handle, 0xFE, prescale);
      80                 :           0 :     i2cWriteByteData(motor_handle, 0x00, oldmode);
      81                 :           0 :     usleep(5000);
      82                 :           0 :     i2cWriteByteData(motor_handle, 0x00, oldmode | 0xA1);
      83                 :           0 : }
      84                 :             : 
      85                 :           0 : CarMove::~CarMove() {
      86                 :           0 :     i2cClose(servo_handle);
      87                 :           0 :     i2cClose(motor_handle);
      88                 :           0 :     gpioTerminate();
      89                 :           0 : }
      90                 :             : 
      91                 :           0 : void CarMove::sequence() {
      92                 :           0 :     setMotorSpeed(100);
      93                 :           0 :     sleep(2);
      94                 :             : 
      95                 :           0 :     setMotorSpeed(-100);
      96                 :           0 :     sleep(2);
      97                 :             : 
      98                 :           0 :     setMotorSpeed(0);
      99                 :             : 
     100                 :           0 :     setServoAngle(-45);
     101                 :           0 :     sleep(1);
     102                 :             : 
     103                 :           0 :     setServoAngle(45);
     104                 :           0 :     sleep(1);
     105                 :             : 
     106                 :           0 :     setServoAngle(0);
     107                 :           0 :     setMotorSpeed(0);
     108                 :           0 : }