Micon Car Rally / Mbed 2 deprecated Servo_Control

This program is controlling the servo motor by MTU2(PWM mode 1).

Dependencies:   mbed

main.cpp@0:7c77a2e078e4, 2016-09-02 (annotated)

Committer:
TetsuyaKonno
Date:
Fri Sep 02 07:16:05 2016 +0000
Revision:
0:7c77a2e078e4
First program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TetsuyaKonno 0:7c77a2e078e4 1 //------------------------------------------------------------------//
TetsuyaKonno 0:7c77a2e078e4 2 //Supported MCU: RZ/A1H
TetsuyaKonno 0:7c77a2e078e4 3 //File Contents: Servo Control (GR-PEACH version on the Micon Car)
TetsuyaKonno 0:7c77a2e078e4 4 //Version number: Ver.1.00
TetsuyaKonno 0:7c77a2e078e4 5 //Date: 2016.01.20
TetsuyaKonno 0:7c77a2e078e4 6 //Copyright: Renesas Electronics Corporation
TetsuyaKonno 0:7c77a2e078e4 7 //------------------------------------------------------------------//
TetsuyaKonno 0:7c77a2e078e4 8
TetsuyaKonno 0:7c77a2e078e4 9 //This program supports the following boards:
TetsuyaKonno 0:7c77a2e078e4 10 //* GR-PEACH(E version)
TetsuyaKonno 0:7c77a2e078e4 11 //* Motor drive board Ver.5
TetsuyaKonno 0:7c77a2e078e4 12 //* Camera module (SC-310)
TetsuyaKonno 0:7c77a2e078e4 13
TetsuyaKonno 0:7c77a2e078e4 14 //Include
TetsuyaKonno 0:7c77a2e078e4 15 //------------------------------------------------------------------//
TetsuyaKonno 0:7c77a2e078e4 16 #include "mbed.h"
TetsuyaKonno 0:7c77a2e078e4 17 #include "iodefine.h"
TetsuyaKonno 0:7c77a2e078e4 18
TetsuyaKonno 0:7c77a2e078e4 19 //Define
TetsuyaKonno 0:7c77a2e078e4 20 //------------------------------------------------------------------//
TetsuyaKonno 0:7c77a2e078e4 21 //Servo PWM cycle
TetsuyaKonno 0:7c77a2e078e4 22 #define SERVO_PWM_CYCLE 33332 /* SERVO PWM period */
TetsuyaKonno 0:7c77a2e078e4 23 /* 16ms P0φ/16 = 0.48us */
TetsuyaKonno 0:7c77a2e078e4 24 #define SERVO_CENTER 3124 /* 1.5ms / 0.48us - 1 = 3124*/
TetsuyaKonno 0:7c77a2e078e4 25 #define HANDLE_STEP 18 /* 1 degree value */
TetsuyaKonno 0:7c77a2e078e4 26
TetsuyaKonno 0:7c77a2e078e4 27 //Constructor
TetsuyaKonno 0:7c77a2e078e4 28 //------------------------------------------------------------------//
TetsuyaKonno 0:7c77a2e078e4 29 Ticker interrput;
TetsuyaKonno 0:7c77a2e078e4 30
TetsuyaKonno 0:7c77a2e078e4 31 //Prototype
TetsuyaKonno 0:7c77a2e078e4 32 //------------------------------------------------------------------//
TetsuyaKonno 0:7c77a2e078e4 33 void init_MTU2_PWM_Servo( void ); /* Initialize PWM functions */
TetsuyaKonno 0:7c77a2e078e4 34 void intTimer( void ); /* Interrupt fanction */
TetsuyaKonno 0:7c77a2e078e4 35 void timer( unsigned long timer_set );
TetsuyaKonno 0:7c77a2e078e4 36 void handle( int angle );
TetsuyaKonno 0:7c77a2e078e4 37
TetsuyaKonno 0:7c77a2e078e4 38 //Globle
TetsuyaKonno 0:7c77a2e078e4 39 //------------------------------------------------------------------//
TetsuyaKonno 0:7c77a2e078e4 40 volatile unsigned long cnt_timer; /* Used by timer function */
TetsuyaKonno 0:7c77a2e078e4 41
TetsuyaKonno 0:7c77a2e078e4 42 //Main
TetsuyaKonno 0:7c77a2e078e4 43 //------------------------------------------------------------------//
TetsuyaKonno 0:7c77a2e078e4 44 int main( void )
TetsuyaKonno 0:7c77a2e078e4 45 {
TetsuyaKonno 0:7c77a2e078e4 46 /* Initialize MCU functions */
TetsuyaKonno 0:7c77a2e078e4 47 init_MTU2_PWM_Servo();
TetsuyaKonno 0:7c77a2e078e4 48 interrput.attach(&intTimer, 0.001);
TetsuyaKonno 0:7c77a2e078e4 49
TetsuyaKonno 0:7c77a2e078e4 50 while(1) {
TetsuyaKonno 0:7c77a2e078e4 51 handle( 0 );
TetsuyaKonno 0:7c77a2e078e4 52 timer( 1000 );
TetsuyaKonno 0:7c77a2e078e4 53 handle( 30 );
TetsuyaKonno 0:7c77a2e078e4 54 timer( 1000 );
TetsuyaKonno 0:7c77a2e078e4 55 handle( 0 );
TetsuyaKonno 0:7c77a2e078e4 56 timer( 1000 );
TetsuyaKonno 0:7c77a2e078e4 57 handle( -30 );
TetsuyaKonno 0:7c77a2e078e4 58 timer( 1000 );
TetsuyaKonno 0:7c77a2e078e4 59 }
TetsuyaKonno 0:7c77a2e078e4 60 }
TetsuyaKonno 0:7c77a2e078e4 61
TetsuyaKonno 0:7c77a2e078e4 62 //Initialize MTU2 PWM functions
TetsuyaKonno 0:7c77a2e078e4 63 //------------------------------------------------------------------//
TetsuyaKonno 0:7c77a2e078e4 64 //MTU2_0
TetsuyaKonno 0:7c77a2e078e4 65 //PWM mode 1
TetsuyaKonno 0:7c77a2e078e4 66 //TIOC0A(P4_0) :Servo-motor
TetsuyaKonno 0:7c77a2e078e4 67 //------------------------------------------------------------------//
TetsuyaKonno 0:7c77a2e078e4 68 void init_MTU2_PWM_Servo( void )
TetsuyaKonno 0:7c77a2e078e4 69 {
TetsuyaKonno 0:7c77a2e078e4 70 /* Port setting for S/W I/O Contorol */
TetsuyaKonno 0:7c77a2e078e4 71 /* alternative mode */
TetsuyaKonno 0:7c77a2e078e4 72
TetsuyaKonno 0:7c77a2e078e4 73 /* MTU2_0 (P4_0) */
TetsuyaKonno 0:7c77a2e078e4 74 GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/
TetsuyaKonno 0:7c77a2e078e4 75 GPIOPFCAE4 &= 0xfffe; /* The alternative function of a pin */
TetsuyaKonno 0:7c77a2e078e4 76 GPIOPFCE4 &= 0xfffe; /* The alternative function of a pin */
TetsuyaKonno 0:7c77a2e078e4 77 GPIOPFC4 |= 0x0001; /* The alternative function of a pin */
TetsuyaKonno 0:7c77a2e078e4 78 /* 2nd alternative function/output */
TetsuyaKonno 0:7c77a2e078e4 79 GPIOP4 &= 0xfffe; /* */
TetsuyaKonno 0:7c77a2e078e4 80 GPIOPM4 &= 0xfffe; /* p4_0:output */
TetsuyaKonno 0:7c77a2e078e4 81 GPIOPMC4 |= 0x0001; /* P4_0:double */
TetsuyaKonno 0:7c77a2e078e4 82
TetsuyaKonno 0:7c77a2e078e4 83 /* Mosule stop 33(MTU2) canceling */
TetsuyaKonno 0:7c77a2e078e4 84 CPGSTBCR3 &= 0xf7;
TetsuyaKonno 0:7c77a2e078e4 85
TetsuyaKonno 0:7c77a2e078e4 86 /* MTU2_0 (Motor PWM) */
TetsuyaKonno 0:7c77a2e078e4 87 MTU2TCR_0 = 0x22; /* TCNT Clear(TGRA), P0φ/16 */
TetsuyaKonno 0:7c77a2e078e4 88 MTU2TIORH_0 = 0x52; /* TGRA L>H, TGRB H>L */
TetsuyaKonno 0:7c77a2e078e4 89 MTU2TMDR_0 = 0x32; /* TGRC and TGRD = Buff-mode*/
TetsuyaKonno 0:7c77a2e078e4 90 /* PWM-mode1 */
TetsuyaKonno 0:7c77a2e078e4 91 MTU2TCNT_0 = 0; /* TCNT0 Set 0 */
TetsuyaKonno 0:7c77a2e078e4 92 MTU2TGRA_0 = MTU2TGRC_0 = SERVO_PWM_CYCLE;
TetsuyaKonno 0:7c77a2e078e4 93 /* PWM-Cycle(16ms) */
TetsuyaKonno 0:7c77a2e078e4 94 MTU2TGRB_0 = MTU2TGRD_0 = 0; /* Servo-motor(P4_0) */
TetsuyaKonno 0:7c77a2e078e4 95 MTU2TSTR |= 0x01; /* TCNT_0 Start */
TetsuyaKonno 0:7c77a2e078e4 96 }
TetsuyaKonno 0:7c77a2e078e4 97
TetsuyaKonno 0:7c77a2e078e4 98 //Interrupt Timer
TetsuyaKonno 0:7c77a2e078e4 99 //------------------------------------------------------------------//
TetsuyaKonno 0:7c77a2e078e4 100 void intTimer( void )
TetsuyaKonno 0:7c77a2e078e4 101 {
TetsuyaKonno 0:7c77a2e078e4 102 cnt_timer++;
TetsuyaKonno 0:7c77a2e078e4 103 }
TetsuyaKonno 0:7c77a2e078e4 104
TetsuyaKonno 0:7c77a2e078e4 105 //Timer fanction
TetsuyaKonno 0:7c77a2e078e4 106 //------------------------------------------------------------------//
TetsuyaKonno 0:7c77a2e078e4 107 void timer( unsigned long timer_set )
TetsuyaKonno 0:7c77a2e078e4 108 {
TetsuyaKonno 0:7c77a2e078e4 109 cnt_timer = 0;
TetsuyaKonno 0:7c77a2e078e4 110 while( cnt_timer < timer_set );
TetsuyaKonno 0:7c77a2e078e4 111 }
TetsuyaKonno 0:7c77a2e078e4 112
TetsuyaKonno 0:7c77a2e078e4 113 //Handle fanction
TetsuyaKonno 0:7c77a2e078e4 114 //------------------------------------------------------------------//
TetsuyaKonno 0:7c77a2e078e4 115 void handle( int angle )
TetsuyaKonno 0:7c77a2e078e4 116 {
TetsuyaKonno 0:7c77a2e078e4 117 /* When the servo move from left to right in reverse, replace "-" with "+" */
TetsuyaKonno 0:7c77a2e078e4 118 MTU2TGRD_0 = SERVO_CENTER - angle * HANDLE_STEP;
TetsuyaKonno 0:7c77a2e078e4 119 }
TetsuyaKonno 0:7c77a2e078e4 120
TetsuyaKonno 0:7c77a2e078e4 121 //------------------------------------------------------------------//
TetsuyaKonno 0:7c77a2e078e4 122 // End of file
TetsuyaKonno 0:7c77a2e078e4 123 //------------------------------------------------------------------//