void motor_cut(void)
{
pwm_set_duty_cycle(0, 2400);
pwm_set_duty_cycle(1, 2400);
pwm_set_duty_cycle(2, 2400);
pwm_set_duty_cycle(3, 2400);
}
void microbit_music_tick(void) {
if (music_data == NULL) {
// music module not yet imported
return;
}
if (music_data->async_state == ASYNC_MUSIC_STATE_IDLE) {
// nothing to do
return;
}
if (ticks < music_data->async_wait_ticks) {
// need to wait for timeout to expire
return;
}
if (music_data->async_state == ASYNC_MUSIC_STATE_ARTICULATE) {
// turn off output and rest
pwm_set_duty_cycle(music_data->async_pin->name, 0);
music_data->async_wait_ticks = ticks + ARTICULATION_MS;
music_data->async_state = ASYNC_MUSIC_STATE_NEXT_NOTE;
} else if (music_data->async_state == ASYNC_MUSIC_STATE_NEXT_NOTE) {
// play next note
if (music_data->async_notes_index >= music_data->async_notes_len) {
if (music_data->async_loop) {
music_data->async_notes_index = 0;
} else {
music_data->async_state = ASYNC_MUSIC_STATE_IDLE;
microbit_obj_pin_free(music_data->async_pin);
music_data->async_pin = NULL;
return;
}
}
mp_obj_t note;
if (music_data->async_notes_len == 1) {
note = music_data->async_note;
} else {
note = ((mp_obj_t*)music_data->async_note)[music_data->async_notes_index];
}
if (note == mp_const_none) {
// a rest (is this even used anymore?)
pwm_set_duty_cycle(music_data->async_pin->name, 0);
music_data->async_wait_ticks = 60000 / music_data->bpm;
music_data->async_state = ASYNC_MUSIC_STATE_NEXT_NOTE;
} else {
// a note
mp_uint_t note_len;
const char *note_str = mp_obj_str_get_data(note, ¬e_len);
uint32_t delay_on = start_note(note_str, note_len, music_data->async_pin);
music_data->async_wait_ticks = ticks + delay_on;
music_data->async_notes_index += 1;
music_data->async_state = ASYNC_MUSIC_STATE_ARTICULATE;
}
}
}
void pwm_init(void){
*AT91C_PMC_PCER |= (1<<AT91C_ID_PWMC);
*AT91C_PIOA_PDR |= PWM_MASK;
*AT91C_PIOA_BSR |= PWM_MASK;
#if ORIGINAL_FREQ
pwm_configure_clock(0x00000014);
#elif DOUBLED_FREQ
pwm_configure_clock(0x0000060F);
#endif
pwm_configure_channel(0, AT91C_PWMC_CPRE_MCKA, 0, 1);
pwm_configure_channel(1, AT91C_PWMC_CPRE_MCKA, 0, 1);
pwm_configure_channel(2, AT91C_PWMC_CPRE_MCKA, 0, 1);
pwm_configure_channel(3, AT91C_PWMC_CPRE_MCKA, 0, 1);
pwm_set_period(0, 4800);
pwm_set_period(1, 4800);
pwm_set_period(2, 4800);
pwm_set_period(3, 4800);
pwm_set_duty_cycle(0, 0);
pwm_set_duty_cycle(1, 0);
pwm_set_duty_cycle(2, 0);
pwm_set_duty_cycle(3, 0); //here must set 0 first, or the polarity will be wrong
pwm_enable_channel(0);
pwm_enable_channel(1);
pwm_enable_channel(2);
pwm_enable_channel(3);
pwm_set_duty_cycle(0, 2400);
pwm_set_duty_cycle(1, 2400);
pwm_set_duty_cycle(2, 2400);
pwm_set_duty_cycle(3, 2400);
}
// python method PWM.set_duty_cycle(channel, duty_cycle)
static PyObject *py_set_duty_cycle(PyObject *self, PyObject *args, PyObject *kwargs)
{
char key[8];
char *channel;
float duty_cycle = 0.0;
static char *kwlist[] = {"channel", "duty_cycle", NULL};
if (!PyArg_ParseTupleAndKeywords(args, kwargs, "s|f", kwlist, &channel, &duty_cycle))
return NULL;
if (duty_cycle < 0.0 || duty_cycle > 100.0)
{
PyErr_SetString(PyExc_ValueError, "duty_cycle must have a value from 0.0 to 100.0");
return NULL;
}
if (!get_pwm_key(channel, key)) {
PyErr_SetString(PyExc_ValueError, "Invalid PWM key or name.");
return NULL;
}
if (pwm_set_duty_cycle(key, duty_cycle) == -1) {
PyErr_SetString(PyExc_RuntimeError, "You must start() the PWM channel first");
return NULL;
}
Py_RETURN_NONE;
}
static bool test_pwm_get_set_duty_cycle_invalid_index(void)
{
float duty_cycle = 5000.f;
return pwm_set_duty_cycle(3, duty_cycle) == -1
&& pwm_get_duty_cycle(3, &duty_cycle) == -1;
}
static bool test_pwm_get_set_duty_cycle_before_init(void)
{
float duty_cycle = 50.f;
return pwm_set_duty_cycle(MIKROBUS_1, duty_cycle) == -1
&& pwm_get_duty_cycle(MIKROBUS_1, &duty_cycle) == -1;
}
STATIC void wait_async_music_idle(void) {
// wait for the async music state to become idle
while (music_data->async_state != ASYNC_MUSIC_STATE_IDLE) {
// allow CTRL-C to stop the music
if (MP_STATE_VM(mp_pending_exception) != MP_OBJ_NULL) {
music_data->async_state = ASYNC_MUSIC_STATE_IDLE;
pwm_set_duty_cycle(music_data->async_pin->name, 0);
break;
}
}
}
void servo_rotate_to(int id, double angle)
{
if(id > sizeof(servo_pin)/sizeof(servo_pin[0]))
return;
/* valid angle is [-90, 90] */
if((angle < -90.0) || (angle > 90.0))
return;
pwm_set_duty_cycle(servo_pin[id][0], servo_pin[id][1], SERVO_ANGLE_TO_DUTY(angle));
}
void servo_init()
{
int i;
for(i=0;i<sizeof(servo_pin)/sizeof(servo_pin[0]);i++)
{
pwm_stop(servo_pin[i][0], servo_pin[i][1]);
pwm_set_period(servo_pin[i][0], servo_pin[i][1], SERVO_STD_PERIOD);
pwm_set_polarity(servo_pin[i][0], servo_pin[i][1], 1);
pwm_set_duty_cycle(servo_pin[i][0], servo_pin[i][1], SERVO_ANGLE_TO_DUTY(0)); /* keep in the middle */
}
start_servo();
}
static void light_set_lightness(u8_t percentage)
{
#if (!MYNEWT_VAL(USE_NEOPIXEL))
int rc;
uint16_t pwm_val = (uint16_t) (percentage * top_val / 100);
#if MYNEWT_VAL(PWM_0)
rc = pwm_set_duty_cycle(pwm0, 0, pwm_val);
assert(rc == 0);
#endif
#if MYNEWT_VAL(PWM_1)
rc = pwm_set_duty_cycle(pwm1, 0, pwm_val);
assert(rc == 0);
#endif
#if MYNEWT_VAL(PWM_2)
rc = pwm_set_duty_cycle(pwm2, 0, pwm_val);
assert(rc == 0);
#endif
#if MYNEWT_VAL(PWM_3)
rc = pwm_set_duty_cycle(pwm3, 0, pwm_val);
assert(rc == 0);
#endif
#else
int i;
u32_t lightness;
u8_t max_lightness = 0x1f;
lightness = (u8_t) (percentage * max_lightness / 100);
for (i = 0; i < WS2812_NUM_LED; i++) {
neopixel[i] = (lightness | lightness << 8 | lightness << 16);
}
ws2812_write(neopixel);
#endif
}
// python method PWM.ChangeDutyCycle(self, dutycycle)
static PyObject *PWM_ChangeDutyCycle(PWMObject *self, PyObject *args)
{
float dutycycle = 0.0;
if (!PyArg_ParseTuple(args, "f", &dutycycle))
return NULL;
if (dutycycle < 0.0 || dutycycle > 100.0)
{
PyErr_SetString(PyExc_ValueError, "dutycycle must have a value from 0.0 to 100.0");
return NULL;
}
self->dutycycle = dutycycle;
pwm_set_duty_cycle(self->gpio, self->dutycycle);
Py_RETURN_NONE;
}
void fan_task(void)
/*****************************************************************************
* Function : See h-file for specification.
*****************************************************************************/
{
// Set speed if changed
static old_speed;
if(old_speed != ref_speed)
{
pwm_set_duty_cycle(ref_speed);
}
old_speed = ref_speed;
// Not used in this project
// Read the current
// if (ADC_RIS_R && ADC_RIS_INR2) {
// INT16U data = (0x3FF & ADC_SSFIFO2_R);
//
// ADC_ISC_R |= ADC_ISC_IN2;
//
// fan_current = data/4; // Ballpark calculation
//
// fan_current = (12*ref_speed/100)*fan_current;
//
// ADC_PSSI_R |= ADC_PSSI_SS2; // Enable the ADC for next time
// }
// Not used in this project
// Calculate RPM
// static INT8U rpm_calculation = RPM_CALCULATION_INTERVAL;
// if(rpm_calculation == 0)
// {
// fan_rpm = (fan_encoder_counter/2)*(1000/(RPM_CALCULATION_INTERVAL*10))*60; // RPM Calculation
// fan_encoder_counter = 0;
// rpm_calculation = RPM_CALCULATION_INTERVAL;
// } else {
// rpm_calculation--;
// }
}
int setPwmDutyCycle(char *channel, float duty_cycle)
{
char key[8];
int allowed = -1;
clear_error_msg();
if (duty_cycle < 0.0 || duty_cycle > 100.0) {
printf("duty_cycle must have a value from 0.0 to 100.0");
return 0;
}
if (!get_pwm_key(channel, key)) {
printf("Invalid PWM key or name.");
return 0;
}
// Check to see if PWM is allowed on the hardware
// A 1 means we're good to go
allowed = pwm_allowed(key);
if (allowed == -1) {
char err[2000];
snprintf(err, sizeof(err), "Error determining hardware. (%s)", get_error_msg());
return 0;
} else if (allowed == 0) {
char err[2000];
snprintf(err, sizeof(err), "PWM %s not available on current Hardware", key);
return 0;
}
if (pwm_set_duty_cycle(key, duty_cycle) == -1) {
char err[2000];
snprintf(err, sizeof(err), "PWM: %s issue: (%s)", channel, get_error_msg());
return 0;
}
}
int pwm_start(const char *key, float duty, float freq, int polarity)
{
char fragment[18];
char pwm_test_fragment[20];
char pwm_test_path[45];
char period_path[50];
char duty_path[50];
char polarity_path[55];
int period_fd, duty_fd, polarity_fd;
struct pwm_exp *new_pwm, *pwm;
if(!BBB_G(pwm_initialized)) {
initialize_pwm();
}
snprintf(fragment, sizeof(fragment), "bone_pwm_%s", key);
if (!load_device_tree(fragment)) {
//error enabling pin for pwm
return -1;
}
//creates the fragment in order to build the pwm_test_filename, such as "pwm_test_P9_13"
snprintf(pwm_test_fragment, sizeof(pwm_test_fragment), "pwm_test_%s", key);
//finds and builds the pwm_test_path, as it can be variable...
build_path(BBB_G(ocp_dir), pwm_test_fragment, pwm_test_path, sizeof(pwm_test_path));
//create the path for the period and duty
snprintf(period_path, sizeof(period_path), "%s/period", pwm_test_path);
snprintf(duty_path, sizeof(duty_path), "%s/duty", pwm_test_path);
snprintf(polarity_path, sizeof(polarity_path), "%s/polarity", pwm_test_path);
//add period and duty fd to pwm list
if ((period_fd = open(period_path, O_RDWR)) < 0)
return -1;
if ((duty_fd = open(duty_path, O_RDWR)) < 0) {
//error, close already opened period_fd.
close(period_fd);
return -1;
}
if ((polarity_fd = open(polarity_path, O_RDWR)) < 0) {
//error, close already opened period_fd and duty_fd.
close(period_fd);
close(duty_fd);
return -1;
}
// add to list
new_pwm = malloc(sizeof(struct pwm_exp));
if (new_pwm == 0) {
return -1; // out of memory
}
strncpy(new_pwm->key, key, KEYLEN); /* can leave string unterminated */
new_pwm->key[KEYLEN] = '\0'; /* terminate string */
new_pwm->period_fd = period_fd;
new_pwm->duty_fd = duty_fd;
new_pwm->polarity_fd = polarity_fd;
new_pwm->next = NULL;
if (exported_pwms == NULL)
{
// create new list
exported_pwms = new_pwm;
} else {
// add to end of existing list
pwm = exported_pwms;
while (pwm->next != NULL)
pwm = pwm->next;
pwm->next = new_pwm;
}
pwm_set_frequency(key, freq);
pwm_set_polarity(key, polarity);
pwm_set_duty_cycle(key, duty);
return 1;
}
void temp_tick(void)
{
double pid_error = 0;
/* Read and average temperatures */
current_temp[EXTRUDER_0] = read_temp(EXTRUDER_0);
#ifdef EXP_Board
current_temp[HEATED_BED_0] = read_temp(HEATED_BED_0);
extruderBlockTemp = current_temp[HEATED_BED_0];
current_temp_r2c2 = read_R2C2_temp();
#endif
pid_error = target_temp[EXTRUDER_0] - current_temp[EXTRUDER_0];
pterm = config.kp * pid_error;
iterm += (config.ki*pid_error);
//pterm = kp * pid_error;
//iterm += (ki*pid_error);
if(iterm > PID_FUNTIONAL_RANGE){
iterm = PID_FUNTIONAL_RANGE;
}else if(iterm < 0){
iterm = 0;
}
dterm_temp = pid_error - last_error;
dterm = config.kd * dterm_temp;
//dterm = kd * dterm_temp;
output = pterm + iterm + dterm;
//output *= 0.95937 + 0.00907*currenBWSpeed + 0.01662*extruderFanSpeed + 0.000009*currenBWSpeed*currenBWSpeed - 0.000035*currenBWSpeed*extruderFanSpeed - 0.000068*extruderFanSpeed*extruderFanSpeed;
//output += output*config.kVent*extruderFanSpeed;
//output += output*config.kBlower*currenBWSpeed;
output = pterm + iterm + dterm;
/*
#ifdef EXP_Board
double p00 = -0.02242;
double p10 = -0.001512*extruderFanSpeed;
double p01 = 0.01811*currenBWSpeed;
double p20 = 0.0003169*extruderFanSpeed*extruderFanSpeed;
double p11 = -0.00006381*extruderFanSpeed*currenBWSpeed;
double p02 = -0.00008276*currenBWSpeed*currenBWSpeed;
double p30 = -0.000002056*extruderFanSpeed*extruderFanSpeed*extruderFanSpeed;
double p21 = -0.000000008015*currenBWSpeed*extruderFanSpeed*extruderFanSpeed;
double p12 = 0.0000002986*extruderFanSpeed*currenBWSpeed*currenBWSpeed;
double pxy = p00 + p10 + p01 + p20 + p11 + p02 + p30 + p21 + p12;
output = output*(1 + pxy);
#endif
*/
last_error = pid_error;
if(output > 100) {
output = 100;
}else if(output<0 ) {
output = 0;
}
if(target_temp[EXTRUDER_0] == 0){
output = 0;
pterm = 0;
iterm = 0;
dterm = 0;
pid_error = 0;
dterm_temp = 0;
}
pwm_set_duty_cycle(5, output);
pwm_set_enable(5);
}
int pwm_start(const char *pinName, int exportNumber, char *pwmDir, float duty, float freq, int polarity, PwmChannel_t *pwm)
{
char pwm_control_path[PATHLEN];
char pwm_path[PATHLEN]; //pwm channel control folder
char export[2];
int fd;
strncpy(pwm->key, pinName, KEYLEN);
if (set_pinmux(pinName, "pwm") < 0)
{
return -1;
}
//export pwm channel
if ((fd = open("/sys/class/pwm/export", O_WRONLY)) < 0)
{
LogError("pwm open(/sys/class/pwm/export) fail (%s)\n", strerror(errno));
return -1;
}
snprintf(export, 2, "%i", exportNumber);
write(fd,export,1);
close(fd);
snprintf(pwm_path, sizeof(pwm_path), "/sys/class/pwm/%s", pwmDir);
//create the path for period control
snprintf(pwm_control_path, sizeof(pwm_control_path), "%s/period_ns", pwm_path);
if ((pwm->period_fd = open(pwm_control_path, O_WRONLY)) < 0)
{
LogError("pwm open(%s) fail (%s)\n", pwm_control_path, strerror(errno));
return -1;
}
snprintf(pwm_control_path, sizeof(pwm_control_path), "%s/duty_ns", pwm_path);
if ((pwm->duty_fd = open(pwm_control_path, O_WRONLY)) < 0) {
//error, close already opened period_fd.
LogError("pwm open(%s) fail (%s)\n", pwm_control_path, strerror(errno));
close(pwm->period_fd);
return -1;
}
snprintf(pwm_control_path, sizeof(pwm_control_path), "%s/polarity", pwm_path);
if ((pwm->polarity_fd = open(pwm_control_path, O_WRONLY)) < 0) {
LogError("pwm open(%s) fail (%s)\n", pwm_control_path, strerror(errno));
//error, close already opened period_fd and duty_fd.
close(pwm->period_fd);
close(pwm->duty_fd);
return -1;
}
snprintf(pwm_control_path, sizeof(pwm_control_path), "%s/run", pwm_path);
if ((pwm->run_fd = open(pwm_control_path, O_WRONLY)) < 0) {
LogError("pwm open(%s) fail (%s)\n", pwm_control_path, strerror(errno));
//error, close already opened period_fd and duty_fd.
close(pwm->polarity_fd);
close(pwm->period_fd);
close(pwm->duty_fd);
return -1;
}
pwm_set_frequency(pwm, freq);
pwm_set_polarity(pwm, polarity);
pwm_set_duty_cycle(pwm, duty);
pwm_set_run(pwm, 1);
return 0;
}
/*******************************************************************************
* MAIN FUNCTION *
*******************************************************************************/
int main(void)
{
unsigned char angle = 0; // declare a variable to store angle
unsigned char i = 0,j = 0;
unsigned char lx = 0, ly = 0, ry = 0;
unsigned int rx = 0;
// ensure all the hardware port in zero initially
PORTA = 0;
PORTB = 0;
PORTC = 0;
PORTD = 0;
PORTE = 0;
// Initialize the I/O port direction, this must be configured according to circuit
// please refer to PTK40A schematic for details
// TRISX control pin direction, output pin must be configure as '0'
// while input must be configure as '1'
TRISA = 0b00010001;
TRISB = 0b00001111;
TRISC = 0b10010011;
TRISD = 0;
TRISE = 0;
// Initialize ADC.
adc_initialize(); //Ensure pin share with analog is being configured to digital correctly
pwm_initialize();
uart_initialize();
beep(2); //buzzer sound for twice
// PTK40A come SKPS PORT TO USE SKPS
// SKPS, control DC motor speed and RC servo motor
// Please connect SKPS at the "Cytron Starter Kit" section
// Select 9600 as the baud rate on SKPS
// servo require the PCM at around 20ms period
// Please move jumper JP9 to SERVO, JP10 to PWM, JP20 & 21 to DC MOTOR
M1 = 1;
M2 = 0; // drive motor in a direction
while(1) // create an infinite loop
{
lx = uc_skps(p_joy_lx); //obtain left joystick x axis value
ly = uc_skps(p_joy_ly); //obtain left joystick y axis value
rx = uc_skps(p_joy_rx); //obtain right joystick x axis value
ry = uc_skps(p_joy_ry); //obtain right joystick y axis value
//control dc motor using joyleft
if(lx==128 && ly==128)
{
pwm_set_duty_cycle(0);
}
//if(lx!=128 && ly!=128)
else if(ly==0)
{
pwm_set_duty_cycle(1000); //set dc motor speed in differect coordinate
}
else if(lx==0)
{
pwm_set_duty_cycle(350);
}
else if(ly==255)
{
pwm_set_duty_cycle(500);
}
else if(lx==255)
{
pwm_set_duty_cycle(750);
}
//control servo motor using joyright
if(ry==0)
{
SERVO = 1; // Servo pin HIGH
delay_10us(100);
SERVO = 0; // Servo pin LOW
delay_ms(18); // delay for around 18ms
}
else if(rx==0)
{
SERVO = 1; // Servo pin HIGH
delay_10us(130);
SERVO = 0; // Servo pin LOW
delay_ms(18); // delay for around 18ms
}
else if(ry==255)
{
SERVO = 1; // Servo pin HIGH
delay_10us(160);
SERVO = 0; // Servo pin LOW
delay_ms(18); // delay for around 18ms
}
else if(rx==255)
{
SERVO = 1; // Servo pin HIGH
delay_10us(200);
SERVO = 0; // Servo pin LOW
delay_ms(18); // delay for around 18ms
}
}
while(1) continue; // infinite loop to prevent PIC from reset if there is no more program
}
STATIC uint32_t start_note(const char *note_str, size_t note_len, const microbit_pin_obj_t *pin) {
pwm_set_duty_cycle(pin->name, 128);
// [NOTE](#|b)(octave)(:length)
// technically, c4 is middle c, so we'll go with that...
// if we define A as 0 and G as 7, then we can use the following
// array of us periods
// these are the periods of note4 (the octave ascending from middle c) from A->B then C->G
STATIC uint16_t periods_us[] = {2273, 2025, 3822, 3405, 3034, 2863, 2551};
// A#, -, C#, D#, -, F#, G#
STATIC uint16_t periods_sharps_us[] = {2145, 0, 3608, 3214, 0, 2703, 2408};
// we'll represent the note as an integer (A=0, G=6)
// TODO: validate the note
uint8_t note_index = (note_str[0] & 0x1f) - 1;
// TODO: the duration and bpm should be persistent between notes
uint32_t ms_per_tick = (60000 / music_data->bpm) / music_data->ticks;
int8_t octave = 0;
bool sharp = false;
size_t current_position = 1;
// parse sharp or flat
if (current_position < note_len && (note_str[current_position] == '#' || note_str[current_position] == 'b')) {
if (note_str[current_position] == 'b') {
// make sure we handle wrapping round gracefully
if (note_index == 0) {
note_index = 6;
} else {
note_index--;
}
// handle the unusual edge case of Cb
if (note_index == 1) {
octave--;
}
}
sharp = true;
current_position++;
}
// parse the octave
if (current_position < note_len && note_str[current_position] != ':') {
// currently this will only work with a one digit number
// use +=, since the sharp/flat code changes octave to compensate.
music_data->last_octave = (note_str[current_position] & 0xf);
current_position++;
}
octave += music_data->last_octave;
// parse the duration
if (current_position < note_len && note_str[current_position] == ':') {
// I'll make this handle up to two digits for the time being.
current_position++;
if (current_position < note_len) {
music_data->last_duration = note_str[current_position] & 0xf;
current_position++;
if (current_position < note_len) {
music_data->last_duration *= 10;
music_data->last_duration += note_str[current_position] & 0xf;
}
} else {
// technically, this should be a syntax error, since this means
// that no duration has been specified. For the time being,
// we'll let you off :D
}
}
// play the note!
// make the octave relative to octave 4
octave -= 4;
// 18 is 'r' or 'R'
if (note_index < 10) {
uint32_t period;
if (sharp) {
if (octave >= 0) {
period = periods_sharps_us[note_index] >> octave;
}
else {
void Pwm::set_duty_cycle(float duty_cycle)
{
(CheckError)pwm_set_duty_cycle(key_.c_str(), duty_cycle);
}
