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?
User | Revision | Line number | New 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 | //------------------------------------------------------------------// |