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;
}
}
/*
* 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;
}
}
}
int32_t main(void)
{
DDPCONbits.JTAGEN = 0; // Disable the JTAG programming port
/*
SYS_CFG_WAIT_STATES (configures flash wait states from system clock)
SYS_CFG_PB_BUS (configures the PB bus from the system clock)
SYS_CFG_PCACHE (configures the pCache if used)
SYS_CFG_ALL (configures the flash wait states, PB bus, and pCache)*/
/* TODO Add user clock/system configuration code if appropriate. */
SYSTEMConfig(SYS_FREQ, SYS_CFG_ALL);
/*Configure Multivector Interrupt Mode. Using Single Vector Mode
is expensive from a timing perspective, so most applications
should probably not use a Single Vector Mode*/
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
/* TODO <INSERT USER APPLICATION CODE HERE> */
// initialise the IO pins and PWM outputs
TRISD = 0;
pickerBusTRIS = 0x00;
pickerBus = 0x00;
OpenOC1( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0); // vibration motor
OpenOC2( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0); // head led
OpenOC3( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0); // base led
OpenTimer2( T2_ON | T2_PS_1_4 | T2_SOURCE_INT, 1024);
SetDCOC1PWM(0);
SetDCOC2PWM(0);
SetDCOC3PWM(0);
setVac1off;
setVac2off;
USBOutHandle = 0;
USBInHandle = 0;
USBDeviceInit(); //usb_device.c. Initializes USB module SFRs and firmware
USBDeviceAttach();
init_component_picker();
/*while(1)
{
LATBbits.LATB0 = feederXHome;
LATBbits.LATB1 = feederZHome;
//ProcessIO();
}*/
}
int main ( void )
{
// Initialize the processor and peripherals.
if ( InitializeSystem() != TRUE )
{
UART2PrintString( "\r\n\r\nCould not initialize USB Custom Demo App - system. Halting.\r\n\r\n" );
while (1);
}
if ( USBHostInit(0) == TRUE )
{
UART2PrintString( "\r\n\r\n***** USB Custom Demo App Initialized *****\r\n\r\n" );
}
else
{
UART2PrintString( "\r\n\r\nCould not initialize USB Custom Demo App - USB. Halting.\r\n\r\n" );
while (1);
}
btClientData.State = BT_STATE_IDLE;
btClientData.Initialized = FALSE;
mPORTAOutputConfig(0x3);
mPORTAWrite(0x0);
mPORTBOutputConfig(0x10);
// OC 1
PPSOutput(PPS_RP4, PPS_OC1);
//Enable Interrupt
SetPriorityIntOC1(4);
EnableIntOC1;
OpenTimer2(T2_ON | T2_PS_1_8 ,0xffff); //
OpenOC1(OC_IDLE_CON | OC_TIMER2_SRC | OC_PWM_EDGE_ALIGN ,OC_SYNC_TRIG_IN_TMR2,0,0);
SetDCOC1PWM(0xc00,0);
// Main Processing Loop
while (1)
{
BTClientTasks();
// Maintain USB Host State
USBHostTasks();
DelayMs(1);
}
return 0;
} // main
void modem_hal_stop()
{
// Return to rest duty cycle
SetDCOC1PWM(REST_DUTY);
//OCxRS = REST_DUTY;
// Disable playback interrupt
mT2IntEnable(0);
//IEC0bits.T2IE = 0;
// Turn PTT led off
pin_write(43, LOW);
}
/*
* setupPWM
*/
int setupPWM(unsigned int fpb)
{
UINT32 pr2;
unsigned int prescalar = 1;
pr2 = (UINT32)((fpb/(100*prescalar)) - 1);
/*PWM resolution [bits] = log2(peripheral bus frequency / (PWM frequency*prescalar)
*e.g. fpb = 40MHz, PWM_FREQUENCY = 100Hz, prescalar = 1 ==> resolution = 18.6 bits
*INFO: The lower PWM frequency, the highter the PWM resolution - BUT: The lower the
*PWM frequency, the higher the ripple after the low-pass filter .
*/
OpenOC1( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE | OC_TIMER_MODE32, 0, 0);
OpenTimer2( T2_ON | T2_PS_1_1 | T2_SOURCE_INT | T2_32BIT_MODE_ON, pr2);
SetDCOC1PWM(pr2/2); //50% duty cycle for starters
return 0;
}
//Set the motor output, values from -1 to 1
void motorSet(double left, double right, char mode){
int val;
if( mode == MOTOR_MODE_BRAKE )
{
MOTOR_BRAKE_LEFT = 1;
MOTOR_BRAKE_RIGHT = 1;
MOTOR_DIR_LEFT = 1;
MOTOR_DIR_RIGHT = 1;
}
else{
if( left < 0.0 )
{
MOTOR_DIR_LEFT = 1;
MOTOR_BRAKE_LEFT = 0;
}
else
{
MOTOR_DIR_LEFT = 0;
MOTOR_BRAKE_LEFT = 1;
}
if( right < 0.0 )
{
MOTOR_DIR_RIGHT = 1;
MOTOR_BRAKE_RIGHT = 0;
}
else
{
MOTOR_DIR_RIGHT = 0;
MOTOR_BRAKE_RIGHT = 1;
}
val = fabsf(left) * MOTOR_PWM_PERIOD;
SetDCOC1PWM(val);
val = fabsf(right) * MOTOR_PWM_PERIOD;
SetDCOC2PWM(val);
}
}
void modem_hal_output_sample(int phase)
{
SetDCOC1PWM(modem_sine_table[phase]);
//OCxRS = modem_sine_table[phase];
}
/**
* @fn void pwmSet( int val );
* @brief Envoie du rapport cyclique au PWM
* @param val valeur du rapport cyclique entre 1 et T3_tick
*/
void pwmSet( int val ) {
SetDCOC1PWM( val );
}
/********************************************************************
* Function: void ProcessIO(void)
*
* PreCondition: None
*
* Input: None
*
* Output: None
*
* Side Effects: None
*
* Overview: This function is a place holder for other user
* routines. It is a mixture of both USB and
* non-USB tasks.
*
* Note: None
*******************************************************************/
void ProcessIO(void)
{
// User Application USB tasks
if((USBDeviceState < CONFIGURED_STATE)||(USBSuspendControl==1)) return;
//Check if we have received an OUT data packet from the host
if(!HIDRxHandleBusy(USBOutHandle))
{
//We just received a packet of data from the USB host.
//Check the first byte of the packet to see what command the host
//application software wants us to fulfill.
switch (ReceivedDataBuffer[0]) {
case 0x01: // System Commands
switch (ReceivedDataBuffer[1]) {
case 0x01: // System Commands
// Copy any waiting debug text to the send data buffer
ToSendDataBuffer[0] = 0xFF;
// Transmit the response to the host
if (!HIDTxHandleBusy(USBInHandle)) {
USBInHandle = HIDTxPacket(HID_EP, (BYTE*) & ToSendDataBuffer[0], 64);
}
break;
default: // Unknown command received
break;
}
break;
case 0x02: // Feeder Commands
switch (ReceivedDataBuffer[1]) {
case 0x02: // Feeder Status
if (FeederStatus() == 1){
ToSendDataBuffer[0] = 0x01;
}
else{
ToSendDataBuffer[0] = 0x00;
}
// Transmit the response to the host
if (!HIDTxHandleBusy(USBInHandle)) {
USBInHandle = HIDTxPacket(HID_EP, (BYTE*) & ToSendDataBuffer[0], 64);
}
break;
case 0x03: // Go to feeder
if ((ReceivedDataBuffer[2] >= 0) && (ReceivedDataBuffer[2] <= 16)){
moveToPosition(ReceivedDataBuffer[2]);
}
break;
case 0x04: // Picker Up
PickerUp();
break;
case 0x05: // Zero Feeder
ZeroFeeder();
break;
case 0x06: // Picker Down
PickerDown();
break;
case 0x07: // Set Picker Port Output
pickerBusval = ((ReceivedDataBuffer[3] << 8) | ReceivedDataBuffer[2]);
pickerBus = pickerBusval;
break;
case 0x08: // Go to feeder
if ((ReceivedDataBuffer[2] > 0) && (ReceivedDataBuffer[2] <= 16)){
moveToPositionWithoutPick(ReceivedDataBuffer[2]);
}
break;
default: // Unknown command received
break;
}
break;
case 0x03: // Vacuum and Vibration Commands
switch (ReceivedDataBuffer[1]) {
case 0x01: // Vacuum 1 set
if (ReceivedDataBuffer[2] == 0x01){
setVac1on;
}
else{
setVac1off;
}
break;
case 0x02: // Vacuum 2 set
if (ReceivedDataBuffer[2] == 0x01){
setVac2on;
}
else{
setVac2off;
}
break;
case 0x03: // Vibration Motor set
if (ReceivedDataBuffer[2] == 0x01){
SetDCOC1PWM(vibrationmotor_duty_cycle);
vibrationrunning = 1;
}
else{
SetDCOC1PWM(0);
vibrationrunning = 0;
}
break;
case 0x04: // Vacuum 1 status
if (vac1running == 1){
ToSendDataBuffer[0] = 0x01;
}
else{
ToSendDataBuffer[0] = 0x00;
}
// Transmit the response to the host
if (!HIDTxHandleBusy(USBInHandle)) {
USBInHandle = HIDTxPacket(HID_EP, (BYTE*) & ToSendDataBuffer[0], 64);
}
break;
case 0x05: // Vacuum 2 status
if (vac2running == 1){
ToSendDataBuffer[0] = 0x01;
}
else{
ToSendDataBuffer[0] = 0x00;
}
// Transmit the response to the host
if (!HIDTxHandleBusy(USBInHandle)) {
USBInHandle = HIDTxPacket(HID_EP, (BYTE*) & ToSendDataBuffer[0], 64);
}
break;
case 0x06: // Vibration Motor status
if (vibrationrunning == 1){
ToSendDataBuffer[0] = 0x01;
}
else{
ToSendDataBuffer[0] = 0x00;
}
// Transmit the response to the host
if (!HIDTxHandleBusy(USBInHandle)) {
USBInHandle = HIDTxPacket(HID_EP, (BYTE*) & ToSendDataBuffer[0], 64);
}
break;
case 0x07: // Vibration Motor status
vibrationmotor_duty_cycle = ReceivedDataBuffer[2] * 4;
if (vibrationrunning == 1){
SetDCOC1PWM(vibrationmotor_duty_cycle);
}
break;
default: // Unknown command received
break;
}
break;
case 0x04: // LED Commands
switch (ReceivedDataBuffer[1]) {
case 0x01: // LED Base Camera on/off
if (ReceivedDataBuffer[2] == 0x01){
SetDCOC2PWM(led1_duty_cycle);
led1running = 1;
}else{
SetDCOC2PWM(0);
led1running = 0;
}
break;
case 0x02: // LED Base Camera PWM set
led1_duty_cycle = ReceivedDataBuffer[2] * 4;
if (led1running == 1){
SetDCOC2PWM(led1_duty_cycle);
}
break;
case 0x03: // LED Head Camera on/off
if (ReceivedDataBuffer[2] == 0x01){
SetDCOC3PWM(led2_duty_cycle);
led2running = 1;
}else{
SetDCOC3PWM(0);
led2running = 0;
}
break;
case 0x04: // LED Head Camera PWM set
led2_duty_cycle = ReceivedDataBuffer[2] * 4;
if (led2running == 1){
SetDCOC3PWM(led2_duty_cycle);
}
break;
case 0x05: // LED Base Status
if (led1running == 1){
ToSendDataBuffer[0] = 0x01;
}
else{
ToSendDataBuffer[0] = 0x00;
}
// Transmit the response to the host
if (!HIDTxHandleBusy(USBInHandle)) {
USBInHandle = HIDTxPacket(HID_EP, (BYTE*) & ToSendDataBuffer[0], 64);
}
break;
case 0x06: // LED Head Status
if (led2running == 1){
ToSendDataBuffer[0] = 0x01;
}
else{
ToSendDataBuffer[0] = 0x00;
}
// Transmit the response to the host
if (!HIDTxHandleBusy(USBInHandle)) {
USBInHandle = HIDTxPacket(HID_EP, (BYTE*) & ToSendDataBuffer[0], 64);
}
break;
default: // Unknown command received
break;
}
break;
default: // Unknown command received
break;
}
//Re-arm the OUT endpoint, so we can receive the next OUT data packet
//that the host may try to send us.
USBOutHandle = HIDRxPacket(HID_EP, (BYTE*)&ReceivedDataBuffer, 64);
}
}//end ProcessIO
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)) {
void process_cmd(void)
{
char goodcmd[] = "- Command Accepted.\n\n\r\0";
char evilcmd[] = "- BAD Command.\n\n\r\0";
char *r = evilcmd;
if (cmdissued == 1)
{
switch(*rbufptr++)
{
/*** STEPPER COMMANDS ***/
case 's':
putcUART1('s');
if(*rbufptr++ == 't'){
putcUART1('t');
switch(*rbufptr++){
case 's':{
putcUART1('s');
putcUART1(' ');
// sts command received (start)
if(*rbufptr++ == ' '){
// sts parameters [1 or 2]
if(*rbufptr == '1'){
r = goodcmd;stepper_enable1=1;
}else if(*rbufptr++ == '2'){
r = goodcmd;stepper_enable2=1;
}
}
}break;
case 'p':{
putcUART1('p');
// stp command received (stop)
if(*rbufptr++ == ' '){
// stp parameters [1 or 2]
if(*rbufptr == '1'){
r = goodcmd;stepper_enable1=0;
}else if(*rbufptr++ == '2'){
r = goodcmd;stepper_enable2=0;
}
}
}break;
case 'd':{
putcUART1('d');
// std command received (disable)
if(*rbufptr == ' '){
*rbufptr++;
// std parameters [1 or 2]
if(*rbufptr == '1'){
r = goodcmd;disable_stepper1();
}else if(*rbufptr++ == '2'){
r = goodcmd;disable_stepper2();
}
}else{
switch(*rbufptr++){
// stdc command received (direction clockwise)
case 'c':{
if(*rbufptr++ == ' '){
// stdc parameters [1 or 2]
if(*rbufptr == '1'){
r = goodcmd;direction1=1;
}else if(*rbufptr++ == '2'){
r = goodcmd;direction2=1;
}
}
}break;
// stdh command received (direction counter clockwise)
case 'h':{
if(*rbufptr++ == ' '){
// stdh parameters [1 or 2]
if(*rbufptr == '1'){
r = goodcmd;direction1=0;
}else if(*rbufptr++ == '2'){
r = goodcmd;direction2=0;
}
}
}break;
}
}
}break;
case 'm':{
putcUART1('m');
switch(*rbufptr++){
// stmf command received (Full Step Mode)
case 'f':{
if(*rbufptr++ == ' '){
// stdc parameters [1 or 2]
if(*rbufptr == '1'){
r = goodcmd;mode_step1=1;
}else if(*rbufptr++ == '2'){
r = goodcmd;mode_step2=1;
}
}
}break;
// stmh command received (Half Step Mode)
case 'h':{
if(*rbufptr++ == ' '){
// stdh parameters [1 or 2]
if(*rbufptr == '1'){
r = goodcmd;mode_step1=0;
}else if(*rbufptr++ == '2'){
r = goodcmd;mode_step2=0;
}
}
}break;
}
}break;
case 'f':{
putcUART1('f');
r = goodcmd;
}break;
// invalid command
default: r = evilcmd;break;
}
}
break;
/*** DC MOTOR COMMANDS ***/
case 'm':
putcUART1('m');
if(*rbufptr++ == 'd'){
putcUART1('d');
if(*rbufptr++ == 'c'){
putcUART1('c');
switch(*rbufptr++){
case 'r':{
putcUART1('r');
// mdcr command received (start)
if(*rbufptr++ == ' '){
// mdcr parameters []
r = goodcmd; SetDCOC1PWM(*rbufptr);
}
}break;
case 'p':{
putcUART1('p');
// mdcp command received (start)
if(*rbufptr++ == ' '){
// mdcp parameters []
}
}break;
case 'i':{
putcUART1('i');
// mdci command received (start)
if(*rbufptr++ == ' '){
// mdci parameters []
}
}break;
case 'd':{
putcUART1('d');
// mdcd command received (start)
if(*rbufptr++ == ' '){
// mdcd parameters []
}
}break;
}
}
}
break;
}
rbufptr = (char*)bufstt;
wbufptr = rbufptr;
putsUART1((unsigned int *)r);
while(BusyUART1());
cmdissued = 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);
}
}
}
}
// Update motor state.
// MOTOR_STATE dir: MOTOR_STOP, MOTOR_FORWARD, MOTOR_BACKWARD
// uint speed: 0 - 1000
void MotorState(BYTE motor, MOTOR_STATE dir, WORD speed)
{
//debug("M%d d%d s%d\r\n", motor, dir, speed);
if(dir == MOTOR_FORWARD)
{
if(motor == 1)
{
M1_FORWARD_IO = 1;
M1_BACKWARD_IO = 0;
}
else if(motor == 2)
{
M2_FORWARD_IO = 1;
M2_BACKWARD_IO = 0;
}
else if(motor == 3)
{
M3_FORWARD_IO = 1;
M3_BACKWARD_IO = 0;
}
}
else if(dir == MOTOR_BACKWARD)
{
if(motor == 1)
{
M1_FORWARD_IO = 0;
M1_BACKWARD_IO = 1;
}
else if(motor == 2)
{
M2_FORWARD_IO = 0;
M2_BACKWARD_IO = 1;
}
else if(motor == 3)
{
M3_FORWARD_IO = 0;
M3_BACKWARD_IO = 1;
//debug("b");
}
}
else
{
if(motor == 1)
{
M1_FORWARD_IO = 0;
M1_BACKWARD_IO = 0;
}
else if(motor == 2)
{
M2_FORWARD_IO = 0;
M2_BACKWARD_IO = 0;
}
else if(motor == 3)
{
M3_FORWARD_IO = 0;
M3_BACKWARD_IO = 0;
}
}
speed += 1000; // code originally setup to run on pwm pulses 1000-2000
if(speed >= 900 && speed <= 2100)
{
if(motor == 1)
SetDCOC1PWM((int)(speed * MOTOR_SPEED_MULT));
else if(motor == 2)
SetDCOC2PWM((int)(speed * MOTOR_SPEED_MULT));
else if(motor == 3)
SetDCOC3PWM((int)(speed * MOTOR_SPEED_MULT));
//else if(motor == 4)
// SetDCOC4PWM((int)(speed * MOTOR_SPEED_MULT));
}
}
/**
* @fn void pwmSet( int val );
* @brief Envoie du rapport cyclique au PWM
* @param val valeur du rapport cyclique entre 1 et T3_tick
*/
void pwmSet( int val ) {
val = (val*T3_TICK)/100;
SetDCOC1PWM( val );
}
void setpwmR(int a)
{
SetDCOC1PWM(a); // Write new duty cycle
}
void PWM_M0_SetDC(INT16U Duty)
{
SetDCOC1PWM((TMR3_RELOAD * Duty)/65536);
}
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);
}
err_t spp_recv(void *arg, struct rfcomm_pcb *pcb, struct pbuf *p, err_t err)
{
struct pbuf *q = NULL;
char cm1[3];
char cm2[3];
u8_t m;
static int lm1;
static int lm2;
int m1;
int m2;
LWIP_DEBUGF(BT_SPP_DEBUG, ("spp_recv: p->len == %d p->tot_len == %d\n", p->len, p->tot_len));
q = pbuf_alloc(PBUF_RAW, p->tot_len, PBUF_RAM);
if(p->tot_len != 0)
{
u8_t *data = p->payload;
*((u8_t*)q->payload) = *((u8_t *)p->payload);
if(*data == 'm')
{
m = 0x10;
cm1[0] = *(data+2);
cm1[1] = *(data+3);
cm1[2] = 0x00;
m1 = atoi(cm1);
if(m1 == 0){
m |= 0x03;
}else if(*(data+1) == '+'){
m |= 0x01;
}else if(*(data+1) == '-'){
m |= 0x02;
}
lm1 = m1;
cm2[0] = *(data+5);
cm2[1] = *(data+6);
cm2[2] = 0x00;
m2 = atoi(cm2);
if(m2 == 0){
m |= 0x0c;
}else if(*(data+4) == '+'){
m |= 0x04;
}else if(*(data+4) == '-'){
m |= 0x08;
}
lm2 = m2;
LWIP_DEBUGF(BT_SPP_DEBUG, ("spp_recv: m1 %d m2 %d\n", m1, m2));
SetDCOC1PWM((0xffff/10)*m1,0);
SetDCOC2PWM((0xffff/10)*m2,0);
#if defined(__PIC24FJ64GB002__)
mPORTAWrite(m);
#elif defined(__PIC24FJ256GB106__)
mPORTEWrite(m);
#endif
}else{
if(*data == 'a')
{
#if defined(__PIC24FJ64GB002__)
#elif defined(__PIC24FJ256GB106__)
mPORTEWrite(0x1f);
#endif
DelayMs(3);
#if defined(__PIC24FJ64GB002__)
#elif defined(__PIC24FJ256GB106__)
mPORTEWrite(0x15);
#endif
}else if(*data == 's')
{
#if defined(__PIC24FJ64GB002__)
#elif defined(__PIC24FJ256GB106__)
mPORTEWrite(0x1f);
#endif
DelayMs(3);
#if defined(__PIC24FJ64GB002__)
#elif defined(__PIC24FJ256GB106__)
mPORTEWrite(0x1a);
#endif
}else if(*data == 'd')
{
#if defined(__PIC24FJ64GB002__)
#elif defined(__PIC24FJ256GB106__)
mPORTEWrite(0x1f);
#endif
DelayMs(3);
#if defined(__PIC24FJ64GB002__)
#elif defined(__PIC24FJ256GB106__)
mPORTEWrite(0x19);
#endif
}else if(*data == 'f')
{
#if defined(__PIC24FJ64GB002__)
#elif defined(__PIC24FJ256GB106__)
mPORTEWrite(0x1f);
#endif
DelayMs(3);
#if defined(__PIC24FJ64GB002__)
#elif defined(__PIC24FJ256GB106__)
mPORTEWrite(0x16);
#endif
}else if(*data == 'g')
{
#if defined(__PIC24FJ64GB002__)
#elif defined(__PIC24FJ256GB106__)
mPORTEWrite(0x10);
#endif
}else{
}
}
}
if(rfcomm_cl(pcb)) {
rfcomm_uih_credits(pcb, PBUF_POOL_SIZE - rfcomm_remote_credits(pcb), q);
} else {
rfcomm_uih(pcb, rfcomm_cn(pcb), q);
}
pbuf_free(q);
pbuf_free(p);
return ERR_OK;
}
/*******************************************************************************
* TASK: taskStandbyWatchdog
*
* DESCRIPTIONS:
* Watchdog for system standby. Lock the screen after timeout.
*
*******************************************************************************/
void taskStandbyWatchdog (void *pvParameters)
{
xTaskHandle xShutdownWatchdogTask = NULL;
portTickType xTimeoutPeriod;
while (1) {
// Read the stack watermark and record it if it's below threshold.
unsigned portBASE_TYPE uxHighWaterMark = uxTaskGetStackHighWaterMark(NULL);
if (uxHighWaterMark < MIN_STACK_WATERMARK) {
xSystemError.bStackLowError = 1;
// Only log it when the watermark value changed.
static portBASE_TYPE uxPreviousWatermark = 0;
if (uxHighWaterMark != uxPreviousWatermark) {
vLogStackWatermark("Standby Watchdog Task", (unsigned short)uxHighWaterMark);
}
uxPreviousWatermark = uxHighWaterMark;
}
// Kill the shutdown watchdog task if it's running.
if (xShutdownWatchdogTask != NULL) {
vTaskDelete(xShutdownWatchdogTask);
xShutdownWatchdogTask = NULL;
}
// Timeout should be shorter while the screen is still locked.
// Delay a while before we check for the flag to let it get updated. The flag is only updated when the screen is not pressed.
vTaskDelay(100 / portTICK_RATE_MS);
if (xSystemState.bStandby != 0) {
xTimeoutPeriod = SHORT_STANDBY_TIMEOUT - DIM_SCREEN_PERIOD;
}
// Get the timeout period base on auto sleep setting.
else {
switch (eAutoSleep) {
case AUTO_SLEEP_15_SEC: xTimeoutPeriod = (15000 / portTICK_RATE_MS) - DIM_SCREEN_PERIOD; break;
case AUTO_SLEEP_30_SEC: xTimeoutPeriod = (30000 / portTICK_RATE_MS) - DIM_SCREEN_PERIOD; break;
case AUTO_SLEEP_1_MIN: xTimeoutPeriod = (60000 / portTICK_RATE_MS) - DIM_SCREEN_PERIOD; break;
case AUTO_SLEEP_2_MIN: xTimeoutPeriod = (120000 / portTICK_RATE_MS) - DIM_SCREEN_PERIOD; break;
case AUTO_SLEEP_5_MIN: xTimeoutPeriod = (300000 / portTICK_RATE_MS) - DIM_SCREEN_PERIOD; break;
case AUTO_SLEEP_10_MIN: xTimeoutPeriod = (600000 / portTICK_RATE_MS) - DIM_SCREEN_PERIOD; break;
case AUTO_SLEEP_NEVER:
// Create the shutdown watchdog task if auto sleep is disabled.
xTaskCreate(taskShutdownWatchdog, "Shutdown", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY + 2, &xShutdownWatchdogTask);
xTimeoutPeriod = portMAX_DELAY;
break;
}
}
// Turn on the backlight.
SetDCOC1PWM(BLight);
// Wait for touch semaphore.
// Loop back if the screen is touched before timeout.
if (xSemaphoreTake(xTouchSemaphore, xTimeoutPeriod) == pdTRUE) continue;
// Dim the backlight.
SetDCOC1PWM(300);
// Wait for touch semaphore again.
// Loop back if the screen is touched before timeout.
if (xSemaphoreTake(xTouchSemaphore, DIM_SCREEN_PERIOD) == pdTRUE) continue;
// Turn off backlight and lock the screen.
SetDCOC1PWM(0);
vLockScreen();
// Start the auto shutdown task.
xTaskCreate(taskShutdownWatchdog, "Shutdown", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY + 2, &xShutdownWatchdogTask);
// Wait forever until the screen is touched.
xSemaphoreTake(xTouchSemaphore, portMAX_DELAY);
} // End of while(1).
}
