pic32user wrote on Wednesday, May 04, 2011:
Hi I am a newbie to FreeRTOS and is learning about it, I am using FreeRTOS on a PIC32MX460F512L it is supposed to drive a hobby RC servo motor. Earlier I have developed the servo code in a separate control loop program and confirmed that it worked. However when I put them into the RTOS it don’t work, the servo simply don’t move. Also mounted on the servo is an IR sensor connected to an external interrupt pin. To test code, I connected the PIC32 to the PC via USB the program is working, the MPLAB output display window showed the task switching, the various ISRs being entered and the IR sensor (ext interrupt) is working (I coded it to blink an LED when interrupted). So I am not sure what went wrong I was hoping those with more experience can point out my mistakes, code below:
/* MPLAB includes */
#include <p32xxxx.h>
#include <plib.h>
/* FreeRTOS.org includes */
#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"
/* Demo includes */
#include "basic_io.h"
///////////////////////////////////
// **** PIC32 Configuration **** //
///////////////////////////////////
#pragma config FPLLODIV = DIV_1, FPLLMUL = MUL_20, FPLLIDIV = DIV_2, CP = OFF
#pragma config FWDTEN = OFF, FPBDIV = DIV_2, POSCMOD = XT, FNOSC = PRIPLL
///////////////////////////////////
// **** Tasks to be created **** //
///////////////////////////////////
static void vServoSensorTask( void *pvParameters );
static void vMovementTask( void *pvParameters );
////////////////////////////////
// **** Servo Definitions ****//
////////////////////////////////
#define Servo_Min_Pos 30
#define Servo_Mid_Pos 60
#define Servo_Max_Pos 90
#define Servo_Init_Pos Servo_Min_Pos
#define Servo_Dir_Clkwise 1
#define Servo_Dir_CClkwise 0
#define LD1_ON LATBSET = (1<<10)
#define LD2_ON LATBSET = (1<<11)
#define LD1_OFF LATBCLR = (1<<10)
#define LD2_OFF LATBCLR = (1<<11)
////////////////////////////////
// **** Global Variables **** //
////////////////////////////////
unsigned int Servo_Direction = Servo_Dir_Clkwise; //Track servo direction.
signed int servo_step_index = 0 //Track the servo sweep position.
const unsigned int Servo_stepping[13] = {30, 35, 40, 45, //This array controls the degree
50, 55, 60, 65, //of the sweep of the servo.
70, 75, 80, 85, 90};
const char str1[] = "IR Sensor triggered!\n";
const char str2[] = "Timer2 is running...\n";
const char str3[] = "OC1 has triggered!\n";
int flag1 = 0;
int flag2 = 0;
int flag3 = 0;
//////////////////////////////////////////
// **** Interrupt Service Routines **** //
//////////////////////////////////////////
void __ISR(_TIMER_2_VECTOR, ipl2) Timer2Handler(void){
mT2ClearIntFlag(); //Clear timer2 interrupt flag.
switch(Servo_Direction)
{
case Servo_Dir_Clkwise:
servo_step_index += 1;
OC1R = Servo_stepping[servo_step_index];
break;
case Servo_Dir_CClkwise:
servo_step_index -= 1;
OC1R = Servo_stepping[servo_step_index];
break;
}
flag2 = 1;
LATGSET = (1<<12); //Starts servo pulse
}
void __ISR(_OUTPUT_COMPARE_1_VECTOR, ipl7) OC1Handler(void){
LATGCLR = (1<<12); //Ends servo pulse.
switch(servo_step_index)
{
case 0:
Servo_Direction = Servo_Dir_Clkwise;
break;
case 13:
Servo_Direction = Servo_Dir_CClkwise;
break;
}
flag3 = 1;
mOC1ClearIntFlag(); //Clear OC1 interrupt flag.
}
void __ISR(_EXTERNAL_1_VECTOR, ipl7) ExtInt1Handler(void)
{
mINT1ClearIntFlag(); //CLear Ext interrupt 1 flag.
mPORTBToggleBits(BIT_10); //Toggle LD1
flag1 = 1;
}
////////////////////////////////////////////////////////////////
// **** Basic hardware and debug interface configuration **** //
////////////////////////////////////////////////////////////////
void vSetupEnvironment( void );
int main(void)
{
/* Configure both the hardware and the debug interface. */
vSetupEnvironment();
xTaskCreate( vServoSensorTask, "ServoSensor", 240, NULL, 1, NULL );
xTaskCreate( vMovementTask, "Movement", 240, NULL, 1, NULL );
/* Start the scheduler so the created tasks start executing. */
vTaskStartScheduler();
for( ;; );
return 0;
}
static void vServoSensorTask( void *pvParameters )
{
for( ;; )
{
/* To get here the event must have occurred. Process the event (in this
case we just print out a message). */
vPrintString( "ServoSensor Task running...\n" );
if(flag1 == 1)
{
flag1 = 0;
vPrintString(str1);
}
if(flag2 == 1)
{
flag2 = 0;
vPrintString(str2);
}
}
}
static void vMovementTask( void *pvParameters )
{
/* As per most tasks, this task is implemented within an infinite loop. */
for( ;; )
{
vPrintString("Movement Task running...\n");
if(flag2 == 1)
{
vPrintString(str3);
}
}
}
void vSetupEnvironment( void )
{
/* Setup the main clock for maximum performance, and the peripheral clock
to equal the main clock divided by 8. */
SYSTEMConfigPerformance( configCPU_CLOCK_HZ );
mOSCSetPBDIV( OSC_PB_DIV_8 );
PORTDCLR = ((1<<1)|(1<<2)|(1<<6)|(1<<7)); //Output pins
TRISDCLR = ((1<<1)|(1<<2)|(1<<6)|(1<<7));
TRISCSET = ((1<<1)|(1<<2)); //Input pins
TRISDSET = ((1<<9)|(1<<10));
TRISB = 0x0;
//TRISG = 0x0; //Set TRISG as output.
PORTB = 0x0;
PORTG = 0x0; //Set PORTG as off.
mPORTBSetPinsDigitalOut(BIT_10);
mPORTGSetPinsDigitalOut(BIT_12);
mPORTESetPinsDigitalIn(BIT_8); //Set RE8 as digital input for INT1
////////////////////////////////////////////////////////////////////////
// //
// Ext Interrupt 1 //
// //
////////////////////////////////////////////////////////////////////////
ConfigINT1( EXT_INT_PRI_7| //Set priority 7.
EXT_INT_SUB_PRI_2| //Set subpriority 2.
RISING_EDGE_INT| //Trigger on rising edge.
EXT_INT_ENABLE); //Enable interrupt.
mINT1ClearIntFlag(); //Clear Ext interrupt 1 flag.
////////////////////////////////////////////////////////////////////////
// //
// Timer 2 Setup //
// //
////////////////////////////////////////////////////////////////////////
//Configure Timer 2 using internal clock, 1:256 prescale
OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_256, 1562); //PR1 = 1562, overflows
//every 40ms.
//Set up the timer interrupt with a priority of 2
ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_2);
////////////////////////////////////////////////////////////////////////
// //
// Output Compare module 1 Setup //
// //
////////////////////////////////////////////////////////////////////////
OpenOC1( OC_ON| //Turns on OC1.
OC_TIMER_MODE16| //Use 16-bit timer.
OC_TIMER2_SRC| //Use Timer 2.
OC_HIGH_LOW| //OC2 toggles output.
OC_TOGGLE_PULSE, //Forces OC1 pin high.
0, //OC1RS value.
Servo_Mid_Pos); //OC1R value.
ConfigIntOC1( OC_INT_PRIOR_7| //OC1 interrupt priority is 7.
OC_INT_SUB_PRI_2| //OC1 sub priority is 2.
OC_INT_ON); //OC1 interrupts enabled.
/* Enable global interrupt handling. */
INTEnableSystemMultiVectoredInt();
DBINIT();
}
I also have an asm wrapper file, not sure if I needed it so I just included it
/**********************************************************************
Interrupts used :
- Timer 1, 2 & 3
- Output Compare Module 1
- External Interrupt 1
**********************************************************************/
#include <p32xxxx.h>
#include <sys/asm.h>
#include "ISR_Support.h"
.set nomips16
.set noreorder
/* Below are needed in this wrapper file */
.extern pxCurrentTCB
.extern vTaskSwitchContext
.extern vPortIncrementTick
.extern T1InterruptHandler
.extern Timer2Handler
.extern OC1Handler
.extern ExtInt1Handler
.extern xISRStackTop
/* Below are labels */
.global T1InterruptHandlerWrapper
.global T2InterruptHandlerWrapper
.global OC1nterruptHandlerWrapper
.global ExtInterrupt1HandlerWrapper
/*.global T3InterruptHandlerWrapper*/
/**************************************/
/* Handler Implementation for Timer 1 */
/**************************************/
/*
.section .FreeRTOS, "ax", @progbits
.set noreorder
.set noat
.ent T1InterruptHandlerWrapper
T1InterruptHandlerWrapper:
portSAVE_CONTEXT
jal Robot_Turning
nop
portRESTORE_CONTEXT
.end T1InterruptHandlerWrapper
*/
/**************************************/
/* Handler Implementation for Timer 2 */
/**************************************/
.section .FreeRTOS, "ax", @progbits
.set noreorder
.set noat
.ent T2InterruptHandlerWrapper
T2InterruptHandlerWrapper:
portSAVE_CONTEXT
jal Timer2Handler
nop
portRESTORE_CONTEXT
.end T2InterruptHandlerWrapper
/**************************************/
/* Handler Implementation for Timer 3 */
/**************************************/
/*
.section .FreeRTOS, "ax", @progbits
.set noreorder
.set noat
.ent T3InterruptHandlerWrapper
T3InterruptHandlerWrapper:
portSAVE_CONTEXT
jal T3InterruptHandler
nop
portRESTORE_CONTEXT
.end T3InterruptHandlerWrapper
*/
/**************************************/
/* Handler Implementation for OC 1 */
/**************************************/
.section .FreeRTOS, "ax", @progbits
.set noreorder
.set noat
.ent OC1InterruptHandlerWrapper
OC1InterruptHandlerWrapper:
portSAVE_CONTEXT
jal OC1Handler
nop
portRESTORE_CONTEXT
.end OC1InterruptHandlerWrapper
/****************************************/
/* Handler Implementation for Ext Int 1 */
/****************************************/
.section .FreeRTOS, "ax", @progbits
.set noreorder
.set noat
.ent ExtInterrupt1HandlerWrapper
ExtInterrupt1HandlerWrapper:
portSAVE_CONTEXT
jal ExtInt1Handler
nop
portRESTORE_CONTEXT
.end ExtInterrupt1HandlerWrapper
By the way only timer2, OC1 and ext interrupt 1 is used at the moment.
Thanks in advance.
Regards,
Kim