<commit_before>#include "common.h"
#include "pin.h"
#include "i2c.h"
#include "servo.h"

void init_servo_driver(_SERVODRIVER *self, _I2C *bus, uint8_t hardware_addr) {

}

void init_servo(_SERVODRIVER *sd, uint8_t number) {

}


void servo_driver_reset(_SERVODRIVER *self) {
    servo_driver_begin_transmission(self, I2C_WRITE);
    servo_driver_write_register(self, PCA9685_MODE1, 0x0);
    servo_driver_end_transmission(self);
}

void servo_driver_set_pwm_freq(_SERVODRIVER *self, float new_freq) {
    freq *= 0.9;  // Correct for overshoot in the frequency setting (see issue #11).
    float prescaleval = 25000000;
    prescaleval /= 4096;
    prescaleval /= freq;
    prescaleval -= 1;
    if (ENABLE_DEBUG_OUTPUT) {
      //Serial.print("Estimated pre-scale: "); Serial.println(prescaleval);
    }
    uint8_t prescale = floor(prescaleval + 0.5);
    if (ENABLE_DEBUG_OUTPUT) {
      //Serial.print("Final pre-scale: "); Serial.println(prescale);
    }
    
    uint8_t oldmode = read8(PCA9685_MODE1);
    uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep
    write8(PCA9685_MODE1, newmode); // go to sleep
    write8(PCA9685_PRESCALE, prescale); // set the prescaler
    write8(PCA9685_MODE1, oldmode);
    delay(5);
    write8(PCA9685_MODE1, oldmode | 0xa1);  //  This sets the MODE1 register to turn on auto increment.
                                            // This is why the beginTransmission below was not working.
    //  Serial.print("Mode now 0x"); Serial.println(read8(PCA9685_MODE1), HEX);
}


void servo_driver_begin_transmission(_SERVODRIVER *dest, uint8_t rw) {
    /*
    Addresses the specified servo driver over I2C. This should be followed by 
    the writing of registers/other commands to this servo driver. rw specifies
    whether the following operations will be "read" (rw=1) or "write" (rw=0)
    operations.
    */
    i2c_start(dest -> bus);
    uint8_t assembled_addr = (dest -> i2c_addr << 1) + rw;
    i2c_putc(dest -> bus, assembled_addr);
}

void servo_driver_write_register(_SERVODRIVER *dest, uint8_t reg, uint8_t val) {
    i2c_putc(dest -> bus, reg);
    i2c_putc(dest -> bus, val);
}

void servo_driver_end_transmission (_SERVODRIVER *dest) {
    i2c_stop(dest -> bus);
}<commit_msg>Added wake, sleep, and PWM frequency adjustment functions to servo library.<commit_after>#include <p24FJ128GB206.h>
#include <libpic30.h>
#include "common.h"
#include "pin.h"
#include "i2c.h"
#include "servo.h"

void init_servo_driver(_SERVODRIVER *self, _I2C *bus, uint8_t hardware_addr) {

}

void init_servo(_SERVODRIVER *sd, uint8_t number) {

}


void servo_driver_sleep(_SERVODRIVER *self) {
    /*
    Puts servo driver into low-power sleep mode (to wake, use wake()) and
    stores mode1 settings to self -> mode1 for later reinstatement on wake.
    */
    servo_driver_begin_transmission(self, I2C_READ);
    self -> mode1 = servo_driver_read_register(self, PCA9685_MODE1);
    uint8_t sleep_mode = (self -> mode1 & 0x7F) | 0x10;
    servo_driver_end_transmission(self);
    // Put servo driver to sleep
    servo_driver_begin_transmission(self, I2C_WRITE);
    servo_driver_write_register(self, PCA9685_MODE1, sleep_mode);
    servo_driver_end_transmission(self)
}

void servo_driver_wake(_SERVODRIVER *self) {
    /*
    Wakes servo driver from sleep (to sleep, use servo_driver_sleep()) and
    reinstates mode1 settings from pre-sleep period (uses self -> mode1).
    */

    servo_driver_begin_transmission(self, I2C_WRITE);
    servo_driver_write_register(self, PCA9685_MODE1, self -> mode1);
    servo_driver_end_transmission(self)
    __delay_us(500); // Oscillator takes max 500us to restart from sleep
    servo_driver_begin_transmission(self, I2C_WRITE);
    servo_driver_write_register(PCA9685_MODE1, self -> mode1 | 0x81);
    servo_driver_end_transmission(self);
    // Servo driver is awake and functioning again!
}

void servo_driver_reset(_SERVODRIVER *self) {
    servo_driver_begin_transmission(self, I2C_WRITE);
    servo_driver_write_register(self, PCA9685_MODE1, 0x0);
    servo_driver_end_transmission(self);
}

void servo_driver_set_pwm_freq(_SERVODRIVER *self, float new_freq) {
    freq *= 0.9;  // Correct for overshoot in the frequency setting (see issue #11).
    float prescaleval = 25000000;
    prescaleval /= 4096;
    prescaleval /= freq;
    prescaleval -= 1;
    uint8_t prescale = floor(prescaleval + 0.5);
    // PWM frequency can only be updated when servo driver is asleep.
    servo_driver_sleep(self);
    servo_driver_begin_transmission(self, I2C_WRITE);
    servo_driver_write_register(PCA9685_PRESCALE, prescale);
    servo_driver_end_transmission(self);
    servo_driver_wake(self);
}


void servo_driver_begin_transmission(_SERVODRIVER *dest, uint8_t rw) {
    /*
    Addresses the specified servo driver over I2C. This should be followed by 
    the writing of registers/other commands to this servo driver. rw specifies
    whether the following operations will be "read" (rw=1) or "write" (rw=0)
    operations.
    */
    i2c_start(dest -> bus);
    uint8_t assembled_addr = (dest -> i2c_addr << 1) + rw;
    i2c_putc(dest -> bus, assembled_addr);
}

void servo_driver_write_register(_SERVODRIVER *dest, uint8_t reg, uint8_t val) {
    i2c_putc(dest -> bus, reg);
    i2c_putc(dest -> bus, val);
}

uint8_t servo_driver_read_register(_SERVODRIVER *dest, uint8_t reg) {
    i2c_putc(dest -> bus, reg);
    return i2c_getc(dest -> bus);
}

void servo_driver_end_transmission (_SERVODRIVER *dest) {
    i2c_stop(dest -> bus);
}
{'fraction_non_alphanumeric': 0.10168569874932028, 'fraction_numerical': 0.03715787565706, 'mean_word_length': 3.975653742110009, 'pattern_counts': {'":': 0, '<': 9, '<?xml version=': 0, '>': 25, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3334575', 'n_tokens_mistral': 2111, 'n_tokens_neox': 1968, 'n_words': 461}