This program is controlling the motor using the PwmOutResetSync library.
Dependencies: PwmOutResetSync mbed
Diff: main.cpp
- Revision:
- 0:605ce817752d
- Child:
- 2:55e672914b8b
diff -r 000000000000 -r 605ce817752d main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Jun 27 07:55:40 2016 +0000 @@ -0,0 +1,176 @@ +//------------------------------------------------------------------// +//Supported MCU: RZ/A1H +//File Contents: Motor Control (GR-PEACH version on the Micon Car) +//Version number: Ver.1.00 +//Date: 2016.01.19 +//Copyright: Renesas Electronics Corporation +//------------------------------------------------------------------// + +//This program supports the following boards: +//* GR-PEACH(E version) +//* Motor drive board Ver.5 +//* Camera module (SC-310) + +//Include +//------------------------------------------------------------------// +#include "mbed.h" +#include "iodefine.h" + +//Define +//------------------------------------------------------------------// +//Motor PWM cycle +#define MOTOR_PWM_CYCLE 33332 /* Motor PWM period */ + /* 1ms P0φ/1 = 0.03us */ +//Motor speed +#define MAX_SPEED 85 /* motor() set: 0 to 100 */ + +//Constructor +//------------------------------------------------------------------// +Ticker interrput; +DigitalOut Left_motor_signal(P4_6); +DigitalOut Right_motor_signal(P4_7); + +//Prototype +//------------------------------------------------------------------// +void init_MTU2_PWM_Motor( void ); /* Initialize PWM functions */ +void intTimer( void ); /* Interrupt fanction */ +void timer( unsigned long timer_set ); +void motor( int accele_l, int accele_r ); +void MTU2_PWM_Motor_Stop( void ); +void MTU2_PWM_Motor_Start( void ); + +//Globle +//------------------------------------------------------------------// +volatile unsigned long cnt_timer; /* Used by timer function */ + +//Main +//------------------------------------------------------------------// +int main( void ) +{ + /* Initialize MCU functions */ + init_MTU2_PWM_Motor(); + interrput.attach(&intTimer, 0.001); + + while(1) { + motor( 100, 0 ); + timer( 1000 ); + motor( 0, 80 ); + timer( 1000 ); + motor( -60, 0 ); + timer( 1000 ); + motor( 0, -40 ); + timer( 1000 ); + motor( 0, 0 ); + timer( 1000 ); + } +} + +//Initialize MTU2 PWM functions +//------------------------------------------------------------------// +//MTU2_3, MTU2_4 +//Reset-Synchronized PWM mode +//TIOC4A(P4_4) :Left-motor +//TIOC4B(P4_5) :Right-motor +//------------------------------------------------------------------// +void init_MTU2_PWM_Motor( void ) +{ + /* Port setting for S/W I/O Contorol */ + /* alternative mode */ + + /* MTU2_4 (P4_4)(P4_5) */ + GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/ + GPIOPFCAE4 &= 0xffcf; /* The alternative function of a pin */ + GPIOPFCE4 |= 0x0030; /* The alternative function of a pin */ + GPIOPFC4 &= 0xffcf; /* The alternative function of a pin */ + /* 2nd altemative function/output */ + GPIOP4 &= 0xffcf; /* */ + GPIOPM4 &= 0xffcf; /* p4_4,P4_5:output */ + GPIOPMC4 |= 0x0030; /* P4_4,P4_5:double */ + + /* Mosule stop 33(MTU2) canceling */ + CPGSTBCR3 &= 0xf7; + + /* MTU2_3 and MTU2_4 (Motor PWM) */ + MTU2TCR_3 = 0x20; /* TCNT Clear(TGRA), P0φ/1 */ + MTU2TOCR1 = 0x04; /* */ + MTU2TOCR2 = 0x40; /* N L>H P H>L */ + MTU2TMDR_3 = 0x38; /* Buff:ON Reset-Synchronized PWM mode */ + MTU2TMDR_4 = 0x30; /* Buff:ON */ + MTU2TOER = 0xc6; /* TIOC3B,4A,4B enabled output */ + MTU2TCNT_3 = MTU2TCNT_4 = 0; /* TCNT3,TCNT4 Set 0 */ + MTU2TGRA_3 = MTU2TGRC_3 = MOTOR_PWM_CYCLE; + /* PWM-Cycle(1ms) */ + MTU2TGRA_4 = MTU2TGRC_4 = 0; /* Left-motor(P4_4) */ + MTU2TGRB_4 = MTU2TGRD_4 = 0; /* Right-motor(P4_5) */ + MTU2TSTR |= 0x40; /* TCNT_4 Start */ +} + +//Interrupt Timer +//------------------------------------------------------------------// +void intTimer( void ) +{ + cnt_timer++; +} + +//Timer fanction +//------------------------------------------------------------------// +void timer( unsigned long timer_set ) +{ + cnt_timer = 0; + while( cnt_timer < timer_set ); +} + +//motor speed control(PWM) +//Arguments: motor:-100 to 100 +//Here, 0 is stop, 100 is forward, -100 is reverse +//------------------------------------------------------------------// +void motor( int accele_l, int accele_r ) +{ + accele_l = ( accele_l * MAX_SPEED ) / 100; + accele_r = ( accele_r * MAX_SPEED ) / 100; + + /* Left Motor Control */ + if( accele_l >= 0 ) { + /* forward */ + Left_motor_signal = 0; + MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_l / 100; + } else { + /* reverse */ + Left_motor_signal = 1; + MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_l ) / 100; + } + + /* Right Motor Control */ + if( accele_r >= 0 ) { + /* forward */ + Right_motor_signal = 0; + MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_r / 100; + } else { + /* reverse */ + Right_motor_signal = 1; + MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_r ) / 100; + } +} + +// MTU2 PWM Motor Stop +//------------------------------------------------------------------// +void MTU2_PWM_Motor_Stop( void ) +{ + MTU2TSTR &= 0xbf; /* TCNT_4 Stop */ + MTU2TOER &= 0xf9; /* TIOC4A,4B enabled output */ + GPIOPMC4 &= 0xffcf; /* P4_4,P4_5 : port mode */ +} + +// MTU2 PWM Motor Start +//------------------------------------------------------------------// +void MTU2_PWM_Motor_Start( void ) +{ + GPIOPMC4 |= 0x0030; /* P4_4, P4_5 : double mode */ + MTU2TOER |= 0xc6; /* TIOC4A,4B enabled output */ + MTU2TCNT_3 = MTU2TCNT_4 = 0; /* TCNT3,TCNT4 Set 0 */ + MTU2TSTR |= 0x40; /* TCNT_3 Start */ +} + +//------------------------------------------------------------------// +// End of file +//------------------------------------------------------------------// \ No newline at end of file