Diff: main.cpp

Revision:

0:605ce817752d

Child:

2:55e672914b8b

--- /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