main.cpp

00001 //------------------------------------------------------------------//
00002 //Supported MCU:   RZ/A1H
00003 //File Contents:   Servo Control (GR-PEACH version on the Micon Car)
00004 //Version number:  Ver.1.00
00005 //Date:            2016.01.20
00006 //Copyright:       Renesas Electronics Corporation
00007 //------------------------------------------------------------------//
00008 
00009 //This program supports the following boards:
00010 //* GR-PEACH(E version)
00011 //* Motor drive board Ver.5
00012 //* Camera module (SC-310)
00013 
00014 //Include
00015 //------------------------------------------------------------------//
00016 #include "mbed.h"
00017 #include "iodefine.h"
00018 
00019 //Define
00020 //------------------------------------------------------------------//
00021 //Servo PWM cycle
00022 #define     SERVO_PWM_CYCLE     33332   /* SERVO PWM period         */
00023                                         /* 16ms   P0φ/16 = 0.48us   */
00024 #define     SERVO_CENTER        3124    /* 1.5ms / 0.48us - 1 = 3124*/
00025 #define     HANDLE_STEP         18      /* 1 degree value           */
00026 
00027 //Constructor
00028 //------------------------------------------------------------------//
00029 Ticker      interrput;
00030 
00031 //Prototype
00032 //------------------------------------------------------------------//
00033 void init_MTU2_PWM_Servo( void );       /* Initialize PWM functions */
00034 void intTimer( void );                  /* Interrupt fanction       */
00035 void timer( unsigned long timer_set );
00036 void handle( int angle );
00037 
00038 //Globle
00039 //------------------------------------------------------------------//
00040 volatile unsigned long  cnt_timer;      /* Used by timer function   */
00041 
00042 //Main
00043 //------------------------------------------------------------------//
00044 int main( void )
00045 {
00046     /* Initialize MCU functions */
00047     init_MTU2_PWM_Servo();
00048     interrput.attach(&intTimer, 0.001);
00049 
00050     while(1) {
00051         handle( 0 );
00052         timer( 1000 );
00053         handle( 30 );
00054         timer( 1000 );
00055         handle( 0 );
00056         timer( 1000 );
00057         handle( -30 );
00058         timer( 1000 );
00059     }
00060 }
00061 
00062 //Initialize MTU2 PWM functions
00063 //------------------------------------------------------------------//
00064 //MTU2_0
00065 //PWM mode 1
00066 //TIOC0A(P4_0) :Servo-motor
00067 //------------------------------------------------------------------//
00068 void init_MTU2_PWM_Servo( void )
00069 {
00070     /* Port setting for S/W I/O Contorol */
00071     /* alternative mode     */
00072 
00073     /* MTU2_0 (P4_0)        */
00074     GPIOPBDC4   = 0x0000;               /* Bidirection mode disabled*/
00075     GPIOPFCAE4 &= 0xfffe;               /* The alternative function of a pin */
00076     GPIOPFCE4  &= 0xfffe;               /* The alternative function of a pin */
00077     GPIOPFC4   |= 0x0001;               /* The alternative function of a pin */
00078                                         /* 2nd alternative function/output   */
00079     GPIOP4     &= 0xfffe;               /*                          */
00080     GPIOPM4    &= 0xfffe;               /* p4_0:output              */
00081     GPIOPMC4   |= 0x0001;               /* P4_0:double              */
00082 
00083     /* Mosule stop 33(MTU2) canceling */
00084     CPGSTBCR3  &= 0xf7;
00085 
00086     /* MTU2_0 (Motor PWM) */
00087     MTU2TCR_0   = 0x22;                 /* TCNT Clear(TGRA), P0φ/16 */
00088     MTU2TIORH_0 = 0x52;                 /* TGRA L>H, TGRB H>L       */
00089     MTU2TMDR_0  = 0x32;                 /* TGRC and TGRD = Buff-mode*/
00090                                         /* PWM-mode1                */
00091     MTU2TCNT_0  = 0;                    /* TCNT0 Set 0              */
00092     MTU2TGRA_0  = MTU2TGRC_0 = SERVO_PWM_CYCLE;
00093                                         /* PWM-Cycle(16ms)          */
00094     MTU2TGRB_0  = MTU2TGRD_0 = 0;       /* Servo-motor(P4_0)        */
00095     MTU2TSTR   |= 0x01;                 /* TCNT_0 Start             */
00096 }
00097 
00098 //Interrupt Timer
00099 //------------------------------------------------------------------//
00100 void intTimer( void )
00101 {
00102     cnt_timer++;
00103 }
00104 
00105 //Timer fanction
00106 //------------------------------------------------------------------//
00107 void timer( unsigned long timer_set )
00108 {
00109     cnt_timer = 0;
00110     while( cnt_timer < timer_set );
00111 }
00112 
00113 //Handle fanction
00114 //------------------------------------------------------------------//
00115 void handle( int angle )
00116 {
00117     /* When the servo move from left to right in reverse, replace "-" with "+" */
00118     MTU2TGRD_0 = SERVO_CENTER - angle * HANDLE_STEP;
00119 }
00120 
00121 //------------------------------------------------------------------//
00122 // End of file
00123 //------------------------------------------------------------------//