Micon Car Rally / Mbed 2 deprecated Motor_Control

This program is controlling the motor by MTU2 (Reset-Synchronized PWM mode).

Dependencies:   mbed

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

Committer:
TetsuyaKonno
Date:
Fri Sep 02 06:42:45 2016 +0000
Revision:
0:dd69e68b8102
First program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TetsuyaKonno 0:dd69e68b8102 1 //------------------------------------------------------------------//
TetsuyaKonno 0:dd69e68b8102 2 //Supported MCU: RZ/A1H
TetsuyaKonno 0:dd69e68b8102 3 //File Contents: Motor Control (GR-PEACH version on the Micon Car)
TetsuyaKonno 0:dd69e68b8102 4 //Version number: Ver.1.00
TetsuyaKonno 0:dd69e68b8102 5 //Date: 2016.01.19
TetsuyaKonno 0:dd69e68b8102 6 //Copyright: Renesas Electronics Corporation
TetsuyaKonno 0:dd69e68b8102 7 //------------------------------------------------------------------//
TetsuyaKonno 0:dd69e68b8102 8
TetsuyaKonno 0:dd69e68b8102 9 //This program supports the following boards:
TetsuyaKonno 0:dd69e68b8102 10 //* GR-PEACH(E version)
TetsuyaKonno 0:dd69e68b8102 11 //* Motor drive board Ver.5
TetsuyaKonno 0:dd69e68b8102 12 //* Camera module (SC-310)
TetsuyaKonno 0:dd69e68b8102 13
TetsuyaKonno 0:dd69e68b8102 14 //Include
TetsuyaKonno 0:dd69e68b8102 15 //------------------------------------------------------------------//
TetsuyaKonno 0:dd69e68b8102 16 #include "mbed.h"
TetsuyaKonno 0:dd69e68b8102 17 #include "iodefine.h"
TetsuyaKonno 0:dd69e68b8102 18
TetsuyaKonno 0:dd69e68b8102 19 //Define
TetsuyaKonno 0:dd69e68b8102 20 //------------------------------------------------------------------//
TetsuyaKonno 0:dd69e68b8102 21 //Motor PWM cycle
TetsuyaKonno 0:dd69e68b8102 22 #define MOTOR_PWM_CYCLE 33332 /* Motor PWM period */
TetsuyaKonno 0:dd69e68b8102 23 /* 1ms P0φ/1 = 0.03us */
TetsuyaKonno 0:dd69e68b8102 24 //Motor speed
TetsuyaKonno 0:dd69e68b8102 25 #define MAX_SPEED 85 /* motor() set: 0 to 100 */
TetsuyaKonno 0:dd69e68b8102 26
TetsuyaKonno 0:dd69e68b8102 27 //Constructor
TetsuyaKonno 0:dd69e68b8102 28 //------------------------------------------------------------------//
TetsuyaKonno 0:dd69e68b8102 29 Ticker interrput;
TetsuyaKonno 0:dd69e68b8102 30 DigitalOut Left_motor_signal(P4_6);
TetsuyaKonno 0:dd69e68b8102 31 DigitalOut Right_motor_signal(P4_7);
TetsuyaKonno 0:dd69e68b8102 32
TetsuyaKonno 0:dd69e68b8102 33 //Prototype
TetsuyaKonno 0:dd69e68b8102 34 //------------------------------------------------------------------//
TetsuyaKonno 0:dd69e68b8102 35 void init_MTU2_PWM_Motor( void ); /* Initialize PWM functions */
TetsuyaKonno 0:dd69e68b8102 36 void intTimer( void ); /* Interrupt fanction */
TetsuyaKonno 0:dd69e68b8102 37 void timer( unsigned long timer_set );
TetsuyaKonno 0:dd69e68b8102 38 void motor( int accele_l, int accele_r );
TetsuyaKonno 0:dd69e68b8102 39 void MTU2_PWM_Motor_Stop( void );
TetsuyaKonno 0:dd69e68b8102 40 void MTU2_PWM_Motor_Start( void );
TetsuyaKonno 0:dd69e68b8102 41
TetsuyaKonno 0:dd69e68b8102 42 //Globle
TetsuyaKonno 0:dd69e68b8102 43 //------------------------------------------------------------------//
TetsuyaKonno 0:dd69e68b8102 44 volatile unsigned long cnt_timer; /* Used by timer function */
TetsuyaKonno 0:dd69e68b8102 45
TetsuyaKonno 0:dd69e68b8102 46 //Main
TetsuyaKonno 0:dd69e68b8102 47 //------------------------------------------------------------------//
TetsuyaKonno 0:dd69e68b8102 48 int main( void )
TetsuyaKonno 0:dd69e68b8102 49 {
TetsuyaKonno 0:dd69e68b8102 50 /* Initialize MCU functions */
TetsuyaKonno 0:dd69e68b8102 51 init_MTU2_PWM_Motor();
TetsuyaKonno 0:dd69e68b8102 52 interrput.attach(&intTimer, 0.001);
TetsuyaKonno 0:dd69e68b8102 53
TetsuyaKonno 0:dd69e68b8102 54 while(1) {
TetsuyaKonno 0:dd69e68b8102 55 motor( 100, 0 );
TetsuyaKonno 0:dd69e68b8102 56 timer( 1000 );
TetsuyaKonno 0:dd69e68b8102 57 motor( 0, 80 );
TetsuyaKonno 0:dd69e68b8102 58 timer( 1000 );
TetsuyaKonno 0:dd69e68b8102 59 motor( -60, 0 );
TetsuyaKonno 0:dd69e68b8102 60 timer( 1000 );
TetsuyaKonno 0:dd69e68b8102 61 motor( 0, -40 );
TetsuyaKonno 0:dd69e68b8102 62 timer( 1000 );
TetsuyaKonno 0:dd69e68b8102 63 motor( 0, 0 );
TetsuyaKonno 0:dd69e68b8102 64 timer( 1000 );
TetsuyaKonno 0:dd69e68b8102 65 }
TetsuyaKonno 0:dd69e68b8102 66 }
TetsuyaKonno 0:dd69e68b8102 67
TetsuyaKonno 0:dd69e68b8102 68 //Initialize MTU2 PWM functions
TetsuyaKonno 0:dd69e68b8102 69 //------------------------------------------------------------------//
TetsuyaKonno 0:dd69e68b8102 70 //MTU2_3, MTU2_4
TetsuyaKonno 0:dd69e68b8102 71 //Reset-Synchronized PWM mode
TetsuyaKonno 0:dd69e68b8102 72 //TIOC4A(P4_4) :Left-motor
TetsuyaKonno 0:dd69e68b8102 73 //TIOC4B(P4_5) :Right-motor
TetsuyaKonno 0:dd69e68b8102 74 //------------------------------------------------------------------//
TetsuyaKonno 0:dd69e68b8102 75 void init_MTU2_PWM_Motor( void )
TetsuyaKonno 0:dd69e68b8102 76 {
TetsuyaKonno 0:dd69e68b8102 77 /* Port setting for S/W I/O Contorol */
TetsuyaKonno 0:dd69e68b8102 78 /* alternative mode */
TetsuyaKonno 0:dd69e68b8102 79
TetsuyaKonno 0:dd69e68b8102 80 /* MTU2_4 (P4_4)(P4_5) */
TetsuyaKonno 0:dd69e68b8102 81 GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/
TetsuyaKonno 0:dd69e68b8102 82 GPIOPFCAE4 &= 0xffcf; /* The alternative function of a pin */
TetsuyaKonno 0:dd69e68b8102 83 GPIOPFCE4 |= 0x0030; /* The alternative function of a pin */
TetsuyaKonno 0:dd69e68b8102 84 GPIOPFC4 &= 0xffcf; /* The alternative function of a pin */
TetsuyaKonno 0:dd69e68b8102 85 /* 2nd altemative function/output */
TetsuyaKonno 0:dd69e68b8102 86 GPIOP4 &= 0xffcf; /* */
TetsuyaKonno 0:dd69e68b8102 87 GPIOPM4 &= 0xffcf; /* p4_4,P4_5:output */
TetsuyaKonno 0:dd69e68b8102 88 GPIOPMC4 |= 0x0030; /* P4_4,P4_5:double */
TetsuyaKonno 0:dd69e68b8102 89
TetsuyaKonno 0:dd69e68b8102 90 /* Mosule stop 33(MTU2) canceling */
TetsuyaKonno 0:dd69e68b8102 91 CPGSTBCR3 &= 0xf7;
TetsuyaKonno 0:dd69e68b8102 92
TetsuyaKonno 0:dd69e68b8102 93 /* MTU2_3 and MTU2_4 (Motor PWM) */
TetsuyaKonno 0:dd69e68b8102 94 MTU2TCR_3 = 0x20; /* TCNT Clear(TGRA), P0φ/1 */
TetsuyaKonno 0:dd69e68b8102 95 MTU2TOCR1 = 0x04; /* */
TetsuyaKonno 0:dd69e68b8102 96 MTU2TOCR2 = 0x40; /* N L>H P H>L */
TetsuyaKonno 0:dd69e68b8102 97 MTU2TMDR_3 = 0x38; /* Buff:ON Reset-Synchronized PWM mode */
TetsuyaKonno 0:dd69e68b8102 98 MTU2TMDR_4 = 0x30; /* Buff:ON */
TetsuyaKonno 0:dd69e68b8102 99 MTU2TOER = 0xc6; /* TIOC3B,4A,4B enabled output */
TetsuyaKonno 0:dd69e68b8102 100 MTU2TCNT_3 = MTU2TCNT_4 = 0; /* TCNT3,TCNT4 Set 0 */
TetsuyaKonno 0:dd69e68b8102 101 MTU2TGRA_3 = MTU2TGRC_3 = MOTOR_PWM_CYCLE;
TetsuyaKonno 0:dd69e68b8102 102 /* PWM-Cycle(1ms) */
TetsuyaKonno 0:dd69e68b8102 103 MTU2TGRA_4 = MTU2TGRC_4 = 0; /* Left-motor(P4_4) */
TetsuyaKonno 0:dd69e68b8102 104 MTU2TGRB_4 = MTU2TGRD_4 = 0; /* Right-motor(P4_5) */
TetsuyaKonno 0:dd69e68b8102 105 MTU2TSTR |= 0x40; /* TCNT_4 Start */
TetsuyaKonno 0:dd69e68b8102 106 }
TetsuyaKonno 0:dd69e68b8102 107
TetsuyaKonno 0:dd69e68b8102 108 //Interrupt Timer
TetsuyaKonno 0:dd69e68b8102 109 //------------------------------------------------------------------//
TetsuyaKonno 0:dd69e68b8102 110 void intTimer( void )
TetsuyaKonno 0:dd69e68b8102 111 {
TetsuyaKonno 0:dd69e68b8102 112 cnt_timer++;
TetsuyaKonno 0:dd69e68b8102 113 }
TetsuyaKonno 0:dd69e68b8102 114
TetsuyaKonno 0:dd69e68b8102 115 //Timer fanction
TetsuyaKonno 0:dd69e68b8102 116 //------------------------------------------------------------------//
TetsuyaKonno 0:dd69e68b8102 117 void timer( unsigned long timer_set )
TetsuyaKonno 0:dd69e68b8102 118 {
TetsuyaKonno 0:dd69e68b8102 119 cnt_timer = 0;
TetsuyaKonno 0:dd69e68b8102 120 while( cnt_timer < timer_set );
TetsuyaKonno 0:dd69e68b8102 121 }
TetsuyaKonno 0:dd69e68b8102 122
TetsuyaKonno 0:dd69e68b8102 123 //motor speed control(PWM)
TetsuyaKonno 0:dd69e68b8102 124 //Arguments: motor:-100 to 100
TetsuyaKonno 0:dd69e68b8102 125 //Here, 0 is stop, 100 is forward, -100 is reverse
TetsuyaKonno 0:dd69e68b8102 126 //------------------------------------------------------------------//
TetsuyaKonno 0:dd69e68b8102 127 void motor( int accele_l, int accele_r )
TetsuyaKonno 0:dd69e68b8102 128 {
TetsuyaKonno 0:dd69e68b8102 129 accele_l = ( accele_l * MAX_SPEED ) / 100;
TetsuyaKonno 0:dd69e68b8102 130 accele_r = ( accele_r * MAX_SPEED ) / 100;
TetsuyaKonno 0:dd69e68b8102 131
TetsuyaKonno 0:dd69e68b8102 132 /* Left Motor Control */
TetsuyaKonno 0:dd69e68b8102 133 if( accele_l >= 0 ) {
TetsuyaKonno 0:dd69e68b8102 134 /* forward */
TetsuyaKonno 0:dd69e68b8102 135 Left_motor_signal = 0;
TetsuyaKonno 0:dd69e68b8102 136 MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_l / 100;
TetsuyaKonno 0:dd69e68b8102 137 } else {
TetsuyaKonno 0:dd69e68b8102 138 /* reverse */
TetsuyaKonno 0:dd69e68b8102 139 Left_motor_signal = 1;
TetsuyaKonno 0:dd69e68b8102 140 MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_l ) / 100;
TetsuyaKonno 0:dd69e68b8102 141 }
TetsuyaKonno 0:dd69e68b8102 142
TetsuyaKonno 0:dd69e68b8102 143 /* Right Motor Control */
TetsuyaKonno 0:dd69e68b8102 144 if( accele_r >= 0 ) {
TetsuyaKonno 0:dd69e68b8102 145 /* forward */
TetsuyaKonno 0:dd69e68b8102 146 Right_motor_signal = 0;
TetsuyaKonno 0:dd69e68b8102 147 MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_r / 100;
TetsuyaKonno 0:dd69e68b8102 148 } else {
TetsuyaKonno 0:dd69e68b8102 149 /* reverse */
TetsuyaKonno 0:dd69e68b8102 150 Right_motor_signal = 1;
TetsuyaKonno 0:dd69e68b8102 151 MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_r ) / 100;
TetsuyaKonno 0:dd69e68b8102 152 }
TetsuyaKonno 0:dd69e68b8102 153 }
TetsuyaKonno 0:dd69e68b8102 154
TetsuyaKonno 0:dd69e68b8102 155 // MTU2 PWM Motor Stop
TetsuyaKonno 0:dd69e68b8102 156 //------------------------------------------------------------------//
TetsuyaKonno 0:dd69e68b8102 157 void MTU2_PWM_Motor_Stop( void )
TetsuyaKonno 0:dd69e68b8102 158 {
TetsuyaKonno 0:dd69e68b8102 159 MTU2TSTR &= 0xbf; /* TCNT_4 Stop */
TetsuyaKonno 0:dd69e68b8102 160 MTU2TOER &= 0xf9; /* TIOC4A,4B enabled output */
TetsuyaKonno 0:dd69e68b8102 161 GPIOPMC4 &= 0xffcf; /* P4_4,P4_5 : port mode */
TetsuyaKonno 0:dd69e68b8102 162 }
TetsuyaKonno 0:dd69e68b8102 163
TetsuyaKonno 0:dd69e68b8102 164 // MTU2 PWM Motor Start
TetsuyaKonno 0:dd69e68b8102 165 //------------------------------------------------------------------//
TetsuyaKonno 0:dd69e68b8102 166 void MTU2_PWM_Motor_Start( void )
TetsuyaKonno 0:dd69e68b8102 167 {
TetsuyaKonno 0:dd69e68b8102 168 GPIOPMC4 |= 0x0030; /* P4_4, P4_5 : double mode */
TetsuyaKonno 0:dd69e68b8102 169 MTU2TOER |= 0xc6; /* TIOC4A,4B enabled output */
TetsuyaKonno 0:dd69e68b8102 170 MTU2TCNT_3 = MTU2TCNT_4 = 0; /* TCNT3,TCNT4 Set 0 */
TetsuyaKonno 0:dd69e68b8102 171 MTU2TSTR |= 0x40; /* TCNT_3 Start */
TetsuyaKonno 0:dd69e68b8102 172 }
TetsuyaKonno 0:dd69e68b8102 173
TetsuyaKonno 0:dd69e68b8102 174 //------------------------------------------------------------------//
TetsuyaKonno 0:dd69e68b8102 175 // End of file
TetsuyaKonno 0:dd69e68b8102 176 //------------------------------------------------------------------//