/*
* Sets the value of OCnRS for the specified hbridge.
*/
static void set_speed(uint8_t hbridge_id, uint32_t speed)
{
enum motor_list motor = (enum motor_list)hbridge_id;
if (motor < NUM_MOTORS)
{
switch(Motors[motor].ocn)
{
case 1:
SetDCOC1PWM(speed);
break;
case 2:
SetDCOC2PWM(speed);
break;
case 3:
SetDCOC3PWM(speed);
break;
case 4:
SetDCOC4PWM(speed);
break;
case 5:
SetDCOC5PWM(speed);
break;
default:
break;
}
}
}
// 20kHz PWM signal, duty from 0-1000, pin D3
void initTimer2Interrupt(void)
{
OpenTimer2(T2_ON | T2_PS_1_4, 1000);
OpenOC4(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0);
HBridgeDuty = 0;
SetDCOC4PWM(HBridgeDuty);
}
void setDutyCycle(int outputControlX, int dutyCycle)
{
switch(outputControlX)
{
case 1:
SetDCOC1PWM(dutyCycle);
break;
case 2:
SetDCOC2PWM(dutyCycle);
break;
case 3:
SetDCOC3PWM(dutyCycle);
break;
case 4:
SetDCOC4PWM(dutyCycle);
break;
case 5:
SetDCOC5PWM(dutyCycle);
break;
default:
SetDCOC1PWM(dutyCycle);
break;
}
}
//////////////////////////////////////////////////////////////////////////////////
/// name: WritePWM
/// params: none
/// return: void
/// desc: tells the dc motor what to do based on the state of the motor (_motorState).
/// if the motor state is UP and the tachometer is greater than the predefined
/// bound, the motor is written to at a predefined speed and the tachometer is
/// decremented.
/// if the motor state is DOWN and the tachometer is less than the predefined
/// threshold, the motor is written to and the tachometer is incremented.
/// if the motor state is STOP, the motor is halted.
void WritePWM(void)
{
// if the state of the motor is not STOP, write to the motor, as the
// directional bit was set in InterpretPWM
if ( _motorState != STOP )
{
SetDCOC4PWM(MOTORSPEED);
// if the motor state is DOWN and the tachometer is within
// its bounds, decrement the tach counter
if ( (_motorState == DOWN) && (_faketach < TACH_MAX) )
_faketach++;
// if the motor state is UP and the tachometer is within
// its bounds, increment the tach counter
else if ( (_motorState == UP) && (_faketach > TACH_MIN) )
_faketach--;
// if the tachometer has exceeded its bounds, halt the motor
// and set its state to STOP
else
{
haltArm();
_motorState = _motorGo = STOP;
}
}
// if the state of the motor is STOP, halt the motor
else
{
haltArm();
}
}
int main(void)
{
// initalize global variables
start = 0;
delay1 = 5;
delay2 = 10;
delay3 = 3;
timesec = 0;
startupPIC();
initalizePIC();
setup_counters();
initAnalogInput();
initMagnets();
while(1) {
if(start) {
EMAG2=0;
SetDCOC4PWM(100);
delaysec(delay1);
SetDCOC4PWM(1000);
delaysec(delay2);
SetDCOC4PWM(500);
delaysec(delay3);
EMAG2=1;
SetDCOC4PWM(0);
// EMAG1=0;
// SetDCOC4PWM(900);
// DIR = 0;
// delaysec(delay1);
// SetDCOC4PWM(0);
// delaysec(delay2);
// SetDCOC4PWM(700);
// delaysec(delay1);
// SetDCOC4PWM(1000);
// EMAG1=1;
start=0;
}
}
}
void initPIC() {
SYSTEMConfig(SYS_FREQ, SYS_CFG_ALL);
initLEDs();
initSerialNU32v2();
// Setup and turn off electromagnets
EMAG1 = 0;
EMAG2 = 0;
TRISEbits.TRISE7 = 0;
TRISCbits.TRISC1 = 0;
// Direction Output
DIR = 1;
TRISAbits.TRISA9 = 0;
setup_counters();
CloseADC10();
#define PARAM1 ADC_MODULE_ON | ADC_FORMAT_INTG | ADC_CLK_AUTO | ADC_AUTO_SAMPLING_ON
#define PARAM2 ADC_VREF_AVDD_AVSS | ADC_OFFSET_CAL_DISABLE | ADC_SCAN_ON | ADC_SAMPLES_PER_INT_16 | ADC_ALT_BUF_OFF | ADC_ALT_INPUT_OFF
#define PARAM3 ADC_CONV_CLK_INTERNAL_RC | ADC_SAMPLE_TIME_31
#define PARAM4 ENABLE_AN0_ANA | ENABLE_AN1_ANA | ENABLE_AN2_ANA | ENABLE_AN3_ANA | ENABLE_AN5_ANA | ENABLE_AN15_ANA
OpenADC10(PARAM1, PARAM2, PARAM3, PARAM4, 0);
EnableADC10();
// 20kHz PWM signal, duty from 0-1000, pin D3
OpenTimer2(T2_ON | T2_PS_1_4, MAX_DUTY);
OpenOC4(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0);
SetDCOC4PWM(0);
// 200 Hz ISR
OpenTimer3(T3_ON | T3_PS_1_256, (6250/4 - 1));
//OpenTimer3(T3_ON | T3_PS_1_256, (62500 - 1));
mT3SetIntPriority(1);
mT3ClearIntFlag();
mT3IntEnable(1);
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
INTEnableSystemMultiVectoredInt();
}
int main(void) {
timesec=0;
// set PIC32 to max computing power
SYSTEMConfig(SYS_FREQ, SYS_CFG_ALL);
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
INTEnableSystemMultiVectoredInt();
initLEDs();
LED0 = 1;
LED1 = 1;
initSerialNU32v2();
setup_counters();
CloseADC10();
#define PARAM1 ADC_MODULE_ON | ADC_FORMAT_INTG | ADC_CLK_AUTO | ADC_AUTO_SAMPLING_ON
#define PARAM2 ADC_VREF_AVDD_AVSS | ADC_OFFSET_CAL_DISABLE | ADC_SCAN_ON | ADC_SAMPLES_PER_INT_16 | ADC_ALT_BUF_OFF | ADC_ALT_INPUT_OFF
#define PARAM3 ADC_CONV_CLK_INTERNAL_RC | ADC_SAMPLE_TIME_31
#define PARAM4 ENABLE_AN0_ANA | ENABLE_AN1_ANA | ENABLE_AN2_ANA | ENABLE_AN3_ANA | ENABLE_AN5_ANA | ENABLE_AN15_ANA
OpenADC10( PARAM1, PARAM2, PARAM3, PARAM4,0);
EnableADC10();
// Setup and turn off electromagnets
EMAG1 = 0; EMAG2 = 0;
TRISEbits.TRISE7 = 0; TRISCbits.TRISC1 = 0;
//Direction Output
DIR = 1;
TRISAbits.TRISA9 = 0;
//g-select Outputs
GSEL1 = 0; GSEL2 = 0;
TRISEbits.TRISE2 = 0; TRISCbits.TRISC13= 0;
//0g Inputs
TRISAbits.TRISA0 = 1;
TRISAbits.TRISA4 = 1;
// 20kHz PWM signal, duty from 0-1000, pin D3
OpenTimer2(T2_ON | T2_PS_1_4, 1000);
OpenOC4(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0);
HBridgeDuty = 0;
SetDCOC4PWM(HBridgeDuty);
// 20Hz ISR
OpenTimer3(T3_ON | T3_PS_1_256, 15625);
mT3SetIntPriority(1);
mT3ClearIntFlag();
mT3IntEnable(1);
while(1) {
if(start){
EMAG2=0;
SetDCOC4PWM(100);
delaysec(delay1);
SetDCOC4PWM(1000);
delaysec(delay2);
SetDCOC4PWM(500);
delaysec(delay3);
EMAG2=1;
SetDCOC4PWM(0);
// EMAG1=0;
// SetDCOC4PWM(900);
// DIR = 0;
// delaysec(delay1);
// SetDCOC4PWM(0);
// delaysec(delay2);
// SetDCOC4PWM(700);
// delaysec(delay1);
// SetDCOC4PWM(1000);
// EMAG1=1;
start=0;
}
}
}
/********************************************************************
* Tache: void TaskControl(void *pvParameters)
*
* Overview: Tâche responsable des différents lois de commande.
* Asservissement de position pour la direction, asservissement de
* courant (couple) pour la propulsion et supervision du niveau de
* batterie. Une fois les traitement fais, elle est responsable du
* post des grandeurs intermédaire pour le debug à la tâche d'affichage
*
* Auteur Date Commentaire
*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
* Théou Jean-Baptiste 7 avril 2011 Première version
* Descoubes hugo 16 mai 2011 vs 1.1
*******************************************************************/
void TaskControl(void *pvParameters){
/* asservissement de position */
short sDirMeasure; // Mesure de position (servomoteur). Unsigned integer sur 10 bits
short sDirConsigne; // Consigne de position (servomoteur). Unsigned integer sur 10 bits
short sDirCommand; // Commande de position (servomoteur). entier compris entre 0 et 100, rapport cyclique pour PWM
/* asservissement de courant */
float fPropMeasure; // Mesure image du courant absorbé par la MCC (propulsion).
short sPropConsigne; // Consigne de courant(propulsion). Unsigned integer sur 10 bits
short sPropCommand; // Commande de courant (propulsion). entier compris entre 0 et 100, rapport cyclique pour PWM
short sPowerMeasure; // Mesure du niveau de batterie. Unsigned integer sur 10 bits
char stateControl = PROPULSION_CONTROL;// machnie d'état pour la commande
struct_DebugPrint printData; // Données à afficher (debug)
struct_mesures measuresReceive; // Mesures reçues depuis l'ISR du timer 3
for( ;; ){
/* Récupération des grandeurs de mesures pour les lois de commande */
xQueueReceive( xQueueAcquiData, &measuresReceive, portMAX_DELAY); // fonction bloquante
/* Mise à jour mesures */
sDirMeasure = measuresReceive.sDirMeasure;
fPropMeasure = (float) measuresReceive.sCurrentMeasure / 35.0; // 35 = facteur d'échelle (capteur + conditionneur + ADC)
sPowerMeasure = measuresReceive.sBattMeasure;
/* Mise à jour mesures - Affichage par le réseau */
resultat = sDirMeasure; // Mise à jour var. globale réseau
resultat_courant = measuresReceive.sCurrentMeasure; // Mise à jour var. globale réseau
/* Mise à jour consignes */
/* Protection Vitesse*/
xSemaphoreTake(xSemaphoreVitesse,portMAX_DELAY);
{
sPropConsigne = Vitesse; // récupération var. globale réseau
}
xSemaphoreGive(xSemaphoreVitesse);
/* Protection ConsDir */
xSemaphoreTake(xSemaphoreConsDir,portMAX_DELAY);
{
sDirConsigne = ConsDir; // récupération var. globale réseau
}
xSemaphoreGive(xSemaphoreConsDir);
/* Machine d'état - tâche contrôle/commande/supervision */
switch(stateControl){
case PROPULSION_CONTROL :
/* Protection Consigne courant */
if(sPropConsigne > 100){
sPropConsigne = 100;
}
else if(sPropConsigne < 0){
sPropConsigne = 0;
}
/* Asservissement de courant (couple). Régulation PI (modèle non-linéaire) */
/* Calcul fais avec des flottant - temps de traitement long */
sPropCommand = propulsionCommand (sPropConsigne , fPropMeasure);
case PROPULSION_PWM :
if((fPropMeasure < 0.0) || (fPropMeasure > MAX_CURRENT) ){
sPropCommand = 0;
}
/* Mise à jour rapport cyclique pour module PWM propulsion */
/* Protection au niveau des erreurs de calculs */
if(sPropCommand < 10)
{
sPropCommand = 0;
}
SetDCOC4PWM(ReadPeriod3() * sPropCommand / 100 );
case DIRECTION_CONTROL :
/* Protection Consigne direction */
if(sDirConsigne > HAUT_BUTE){
sDirConsigne = HAUT_BUTE;
}
else if(sDirConsigne < BAS_BUTE){
sDirConsigne = BAS_BUTE;
}
/* Limitation sur la var globale ConsDir fait localement sur le MCU ... pour le moment ! */
/* Protection ConsDir */
xSemaphoreTake(xSemaphoreConsDir,portMAX_DELAY);
{
ConsDir = sDirConsigne;
}
xSemaphoreGive(xSemaphoreConsDir);
/* Asservissement de position. Régulation proporionnelle (modèle grandement non-linéaire) */
sDirCommand = directionCommand(sDirConsigne, sDirMeasure);
case DIRECTION_PWM :
/* Vérification butées direction */
if( sDirMeasure < HAUT_BUTE && sDirMeasure > BAS_BUTE){
if(init_ok == 0){
/* Attendre initialisation utilisateur via réseau */
sDirCommand = 0;
}
else{
/* valeur absolue et sens de rotation - PWM comprise entre 0 et 100 */
if ( sDirCommand < 0 ){
/* Sens de rotation pour le driver de bras de pont */
PORTSetBits(BROCHE_DIR_DIRECTION);
sDirCommand = -sDirCommand;
}
else{
/* Sens de rotation pour le driver de bras de pont */
PORTClearBits(BROCHE_DIR_DIRECTION);
}
/* Protection et limitation Commande */
if(sDirCommand < 20){
sDirCommand = 0; // Pas de commande si dans dead zone
}
else if(sDirCommand < 30){
sDirCommand = 40; // compensation dead zone
}
else if (sDirCommand > 100){
sDirCommand = 100; // limitation PWM
}
}
}
else{
sDirCommand = 0;
}
/* Mise à jour rapport cyclique pour module PWM direction */
SetDCOC2PWM(ReadPeriod3() * sDirCommand / 100 );
case POWER_SUPERVIOR :
break;
default:
break;
}
/* Post les valeurs à afficher pour le debug vers la tâche d'affichage */
printData.commandeDir = sDirCommand;
printData.mesureDir = sDirMeasure;
printData.consigneDir = sDirConsigne;
printData.consigneMove = (float) sPropConsigne;
printData.mesureMove = fPropMeasure;
printData.commandeMove = sPropCommand;
printData.mesurePower = sPowerMeasure;
xQueueSend(xQueueDebugPrint, &printData, 0);
}
}
static PT_THREAD(protothread_CSense(struct pt *pt)) {
PT_BEGIN(pt);
while (1) {
if (config1 & CUR_SENSE_EN) {
if(Status2 & M7_CUR){
if(M7_LastDir != M7_Dir)//if direction has changed, lower CS flag, which enables PWM writing
Status2 &= !M7_CUR;
}
if(Status2 & M6_CUR){
if(M6_LastDir != M6_Dir)
Status2 &= !M6_CUR;
}
if(Status2 & M6_CUR){
if(M6_LastDir != M6_Dir)
Status2 &= !M6_CUR;
}
if(Status2 & M5_CUR){
if(M5_LastDir != M5_Dir)
Status2 &= !M5_CUR;
}
if(Status2 & M4_CUR){
if(M4_LastDir != M4_Dir)
Status2 &= !M4_CUR;
}
if(Status2 & M3_CUR){
if(M3_LastDir != M3_Dir)
Status2 &= !M3_CUR;
}
if (mPORTBReadBits(BIT_3)) {//CS triggered!
M7_PWM = 0;
SetDCOC1PWM(M7_PWM);//Halt motor
Status2 |= M7_CUR;//set CS flag
M7_LastDir = M7_Dir;//save direction
}
if (mPORTBReadBits(BIT_1)) {
M6_PWM = 0;
SetDCOC2PWM(M6_PWM);
Status2 |= M6_CUR;
M6_LastDir = M6_Dir;
}
if (mPORTAReadBits(BIT_3)) {
M5_PWM = 0;
SetDCOC3PWM(M5_PWM);
Status2 |= M5_CUR;
M5_LastDir = M5_Dir;
}
if (mPORTBReadBits(BIT_4)) {
M4_PWM = 0;
SetDCOC5PWM(M4_PWM);
Status2 |= M4_CUR;
M4_LastDir = M4_Dir;
}
if (mPORTAReadBits(BIT_4)) {
M3_PWM = 0;
SetDCOC4PWM(M3_PWM);
Status2 |= M3_CUR;
M3_LastDir = M3_Dir;
}
}
PT_YIELD_TIME_msec(10);
}
PT_END(pt);
}
void __ISR(_I2C_1_VECTOR, ipl3) _SlaveI2CHandler(void) {
unsigned char temp;
static unsigned int dIndex;
// check for MASTER and Bus events and respond accordingly
if (IFS1bits.I2C1MIF) {
mI2C1MClearIntFlag();
return;
}
if (IFS1bits.I2C1BIF) {//bus collision, reset I2C state machine
I2Cstate = 0;
mI2C1BClearIntFlag();
return;
}
// handle the incoming message
if ((I2C1STATbits.R_W == 0) && (I2C1STATbits.D_A == 0)) {
// reset any state variables needed by a message sequence
// perform a dummy read
temp = SlaveReadI2C1();
I2C1CONbits.SCLREL = 1; // release the clock
I2Cstate = 0;
} else if ((I2C1STATbits.R_W == 0) && (I2C1STATbits.D_A == 1)) {//data received, input to slave
WDTCount = 0;
WriteTimer1(0);
// writing data to our module
I2CDataIn = SlaveReadI2C1();
I2C1CONbits.SCLREL = 1; // release clock stretch bit
if (I2Cstate == 0 && I2CDataIn != ADDR_CLR_I2C_STATE) {
I2Cstate = 1;
I2C_request = I2CDataIn;
} else if (I2Cstate == 1) {
switch (I2C_request) {
case ADDR_M3_PWM:
M3_Dir = I2CDataIn >> 7 & 0x01;//take top bit
if (((Status2 & M3_CUR) && (config1 & CUR_SENSE_EN)) || (config1 & MOTOR_HALT)) {
M3_PWM = 0;
} else {
if (I2CDataIn & 0x80) {
mPORTBClearBits(BIT_13);
M3_PWM = (((int) ((~I2CDataIn) + 1)) << 4);
} else {
mPORTBSetBits(BIT_13);
M3_PWM = ((int) I2CDataIn) << 4;
}
}
SetDCOC4PWM(M3_PWM);
break;
case ADDR_M4_PWM:
M4_Dir = I2CDataIn >> 7 & 0x01;//take top bit
if (((Status2 & M4_CUR) && (config1 & CUR_SENSE_EN)) || (config1 & MOTOR_HALT)) {
M4_PWM = 0;
} else {
if (I2CDataIn & 0x80) {
mPORTBClearBits(BIT_14);
M4_PWM = (((int) ((~I2CDataIn) + 1)) << 4);
} else {
mPORTBSetBits(BIT_14);
M4_PWM = ((int) I2CDataIn) << 4;
}
}
SetDCOC5PWM(M4_PWM);
break;
case ADDR_M5_PWM:
M5_Dir = I2CDataIn >> 7 & 0x01;//take top bit
if (((Status2 & M5_CUR) && (config1 & CUR_SENSE_EN)) || (config1 & MOTOR_HALT)) {
M5_PWM = 0;
} else {
if (I2CDataIn & 0x80) {
mPORTBClearBits(BIT_15);
M5_PWM = (((int) ((~I2CDataIn) + 1)) << 4);
} else {
mPORTBSetBits(BIT_15);
M5_PWM = ((int) I2CDataIn) << 4;
}
}
SetDCOC3PWM(M5_PWM);
break;
case ADDR_M6_PWM:
M6_Dir = I2CDataIn >> 7 & 0x01;//take top bit
if (((Status2 & M6_CUR) && (config1 & CUR_SENSE_EN)) || (config1 & MOTOR_HALT)) {
M6_PWM = 0;
} else {
if (I2CDataIn & 0x80) {
mPORTBClearBits(BIT_11);
M6_PWM = (((int) ((~I2CDataIn) + 1)) << 4);
} else {
mPORTBSetBits(BIT_11);
M6_PWM = ((int) I2CDataIn) << 4;
}
}
SetDCOC2PWM(M6_PWM);
break;
case ADDR_M7_PWM:
M7_Dir = I2CDataIn >> 7 & 0x01;//take top bit
if (((Status2 & M7_CUR) && (config1 & CUR_SENSE_EN)) || (config1 & MOTOR_HALT)) {
M7_PWM = 0;
} else {
if (I2CDataIn & 0x80) {
mPORTBClearBits(BIT_7);
M7_PWM = (((int) ((~I2CDataIn) + 1)) << 4);
} else {
mPORTBSetBits(BIT_7);
M7_PWM = ((int) I2CDataIn) << 4;
}
}
SetDCOC1PWM(M7_PWM);
break;
case ADDR_Config1:
config1 = I2CDataIn;
if (config1 & MOTOR_HALT) {
SetDCOC1PWM(0);
SetDCOC2PWM(0);
SetDCOC3PWM(0);
SetDCOC4PWM(0);
SetDCOC5PWM(0);
} else {
SetDCOC1PWM(M7_PWM);
SetDCOC2PWM(M6_PWM);
SetDCOC3PWM(M5_PWM);
SetDCOC4PWM(M3_PWM);
SetDCOC5PWM(M4_PWM);
}
break;
default:
break;
}
I2Cstate = 0;
}
} else if ((I2C1STATbits.R_W == 1) && (I2C1STATbits.D_A == 0)) {
//*********************************************************************
//* PWM output only works on the pins with hardware support.
//* These are defined in the appropriate pins_*.c file.
//* For the rest of the pins, we default to digital output.
//*********************************************************************
void analogWrite(uint8_t pin, int val)
{
// We need to make sure the PWM output is enabled for those pins
// that support it, as we turn it off when digitally reading or
// writing with them. Also, make sure the pin is in output mode
// for consistenty with Wiring, which doesn't require a pinMode
// call for the analog output pins.
pinMode(pin, OUTPUT);
if (val == 0)
{
digitalWrite(pin, LOW);
}
else if (val == 255)
{
digitalWrite(pin, HIGH);
}
else
{
switch(digitalPinToTimer(pin))
{
#ifdef _OCMP1
case TIMER_OC1:
//* Open Timer2 with Period register value
OpenTimer2(T2_ON | T2_PS_1_256, PWM_TIMER_PERIOD);
OpenOC1( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, (PWM_TIMER_PERIOD*val)/256, (PWM_TIMER_PERIOD*val)/256 );
//Set duty cycle on fly
SetDCOC1PWM((PWM_TIMER_PERIOD*val)/256);
break;
#endif
#ifdef _OCMP2
case TIMER_OC2:
//* Open Timer2 with Period register value
OpenTimer2(T2_ON | T2_PS_1_256, PWM_TIMER_PERIOD);
OpenOC2( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, (PWM_TIMER_PERIOD*val)/256, (PWM_TIMER_PERIOD*val)/256 );
//Set duty cycle on fly
SetDCOC2PWM((PWM_TIMER_PERIOD*val)/256);
break;
#endif
#ifdef _OCMP3
case TIMER_OC3:
//* Open Timer2 with Period register value
OpenTimer2(T2_ON | T2_PS_1_256, PWM_TIMER_PERIOD);
OpenOC3( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, (PWM_TIMER_PERIOD*val)/256, (PWM_TIMER_PERIOD*val)/256 );
//Set duty cycle on fly
SetDCOC3PWM((PWM_TIMER_PERIOD*val)/256);
break;
#endif
#ifdef _OCMP4
case TIMER_OC4:
//* Open Timer2 with Period register value
OpenTimer2(T2_ON | T2_PS_1_256, PWM_TIMER_PERIOD);
OpenOC4( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, (PWM_TIMER_PERIOD*val)/256, (PWM_TIMER_PERIOD*val)/256 );
//Set duty cycle on fly
SetDCOC4PWM((PWM_TIMER_PERIOD*val)/256);
break;
#endif
#ifdef _OCMP5
case TIMER_OC5:
//* Open Timer2 with Period register value
OpenTimer2(T2_ON | T2_PS_1_256, PWM_TIMER_PERIOD);
OpenOC5( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, (PWM_TIMER_PERIOD*val)/256, (PWM_TIMER_PERIOD*val)/256 );
//Set duty cycle on fly
SetDCOC5PWM((PWM_TIMER_PERIOD*val)/256);
break;
#endif
#if 0
//* this is the original code, I want to keep it around for refernce for a bit longer
#ifdef _OCMP1
case TIMER_OC1:
//* Open Timer2 with Period register value
OpenTimer2(T2_ON | T2_PS_1_256, PWM_TIMER_PERIOD);
OpenOC1( OC_ON | OC_TIMER_MODE32 | OC_TIMER2_SRC | OC_CONTINUE_PULSE | OC_LOW_HIGH, 256, (256 - val) );
// SetDCOC1PWM((PWM_TIMER_PERIOD * val) / 256);
break;
#endif
#ifdef _OCMP2
case TIMER_OC2:
//* Open Timer2 with Period register value
OpenTimer2(T2_ON | T2_PS_1_256, PWM_TIMER_PERIOD);
OpenOC2( OC_ON | OC_TIMER_MODE32 | OC_TIMER2_SRC | OC_CONTINUE_PULSE | OC_LOW_HIGH, 256, (256 - val) );
// SetDCOC2PWM((PWM_TIMER_PERIOD * val) / 256);
break;
#endif
#ifdef _OCMP3
case TIMER_OC3:
//* Open Timer2 with Period register value
OpenTimer2(T2_ON | T2_PS_1_256, PWM_TIMER_PERIOD);
OpenOC3( OC_ON | OC_TIMER_MODE32 | OC_TIMER2_SRC | OC_CONTINUE_PULSE | OC_LOW_HIGH, 256, (256 - val) );
// SetDCOC3PWM((PWM_TIMER_PERIOD * val) / 256);
break;
#endif
#ifdef _OCMP4
case TIMER_OC4:
//* Open Timer2 with Period register value
OpenTimer2(T2_ON | T2_PS_1_256, PWM_TIMER_PERIOD);
OpenOC4( OC_ON | OC_TIMER_MODE32 | OC_TIMER2_SRC | OC_CONTINUE_PULSE | OC_LOW_HIGH, 256, (256 - val) );
// SetDCOC4PWM((PWM_TIMER_PERIOD * val) / 256);
break;
#endif
#ifdef _OCMP5
case TIMER_OC5:
//* Open Timer2 with Period register value
OpenTimer2(T2_ON | T2_PS_1_256, PWM_TIMER_PERIOD);
OpenOC5( OC_ON | OC_TIMER_MODE32 | OC_TIMER2_SRC | OC_CONTINUE_PULSE | OC_LOW_HIGH, 256, (256 - val) );
// SetDCOC5PWM((PWM_TIMER_PERIOD * val) / 256);
break;
#endif
#endif
case NOT_ON_TIMER:
default:
if (val < 128)
{
digitalWrite(pin, LOW);
}
else
{
digitalWrite(pin, HIGH);
}
}
}
}
//////////////////////////////////////////////////////////////////////////////////
/// name: haltArm
/// params: none
/// return: void
/// desc: write value of STOP to the motor, which is 0, making it stop.
void haltArm(void)
{
SetDCOC4PWM(STOP);
}
