Daiki Kato / Mbed 2 deprecated Motor_Control_using_lib

This program is controlling the motor using the PwmOutResetSync library.

Dependencies:   PwmOutResetSync mbed

main.cpp@0:605ce817752d, 2016-06-27 (annotated)

Committer:
TetsuyaKonno
Date:
Mon Jun 27 07:55:40 2016 +0000
Revision:
0:605ce817752d
Child:
2:55e672914b8b
Motor Control (New program file)

Who changed what in which revision?

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