Micon Car Rally / Mbed 2 deprecated Adjusting_Servo_Center

Servo center adjustment from the PC.

Dependencies:   mbed

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

Committer:
TetsuyaKonno
Date:
Fri Sep 02 06:27:47 2016 +0000
Revision:
0:665a5dc5ab89
first program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TetsuyaKonno 0:665a5dc5ab89 1 //------------------------------------------------------------------//
TetsuyaKonno 0:665a5dc5ab89 2 //Supported MCU: RZ/A1H
TetsuyaKonno 0:665a5dc5ab89 3 //File Contents: Servo center adjustment from the PC
TetsuyaKonno 0:665a5dc5ab89 4 // (GR-PEACH version on the Micon Car)
TetsuyaKonno 0:665a5dc5ab89 5 //Version number: Ver.1.00
TetsuyaKonno 0:665a5dc5ab89 6 //Date: 2016.01.20
TetsuyaKonno 0:665a5dc5ab89 7 //Copyright: Renesas Electronics Corporation
TetsuyaKonno 0:665a5dc5ab89 8 //------------------------------------------------------------------//
TetsuyaKonno 0:665a5dc5ab89 9
TetsuyaKonno 0:665a5dc5ab89 10 //This program supports the following boards:
TetsuyaKonno 0:665a5dc5ab89 11 //* GR-PEACH(E version)
TetsuyaKonno 0:665a5dc5ab89 12 //* Motor drive board Ver.5
TetsuyaKonno 0:665a5dc5ab89 13 //* Camera module (SC-310)
TetsuyaKonno 0:665a5dc5ab89 14
TetsuyaKonno 0:665a5dc5ab89 15 /*
TetsuyaKonno 0:665a5dc5ab89 16 This is a program to check the servo adjustment of the micon car using keys.
TetsuyaKonno 0:665a5dc5ab89 17 The action described takes place when one of the following keys is pressed.
TetsuyaKonno 0:665a5dc5ab89 18
TetsuyaKonno 0:665a5dc5ab89 19 Z key: servo offset value +1
TetsuyaKonno 0:665a5dc5ab89 20 X key: servo offset value -1
TetsuyaKonno 0:665a5dc5ab89 21 A key: servo offset value +10
TetsuyaKonno 0:665a5dc5ab89 22 S key: servo offset value -10
TetsuyaKonno 0:665a5dc5ab89 23 */
TetsuyaKonno 0:665a5dc5ab89 24
TetsuyaKonno 0:665a5dc5ab89 25 //Include
TetsuyaKonno 0:665a5dc5ab89 26 //------------------------------------------------------------------//
TetsuyaKonno 0:665a5dc5ab89 27 #include "mbed.h"
TetsuyaKonno 0:665a5dc5ab89 28 #include "iodefine.h"
TetsuyaKonno 0:665a5dc5ab89 29
TetsuyaKonno 0:665a5dc5ab89 30 //Define
TetsuyaKonno 0:665a5dc5ab89 31 //------------------------------------------------------------------//
TetsuyaKonno 0:665a5dc5ab89 32 //Servo PWM cycle
TetsuyaKonno 0:665a5dc5ab89 33 #define SERVO_PWM_CYCLE 33332 /* SERVO PWM period */
TetsuyaKonno 0:665a5dc5ab89 34 /* 16ms P0φ/16 = 0.48us */
TetsuyaKonno 0:665a5dc5ab89 35 #define SERVO_CENTER 3124 /* 1.5ms / 0.48us - 1 = 3124*/
TetsuyaKonno 0:665a5dc5ab89 36 #define HANDLE_STEP 18 /* 1 degree value */
TetsuyaKonno 0:665a5dc5ab89 37
TetsuyaKonno 0:665a5dc5ab89 38 //Degrees
TetsuyaKonno 0:665a5dc5ab89 39 #define DEGREEES_MAX 4791 /* Left 90° 2.3ms / 0.48us = 4791 */
TetsuyaKonno 0:665a5dc5ab89 40 #define DEGREEES_MIN 1485 /* Right 90° 0.7ms / 0.48us = 1458 */
TetsuyaKonno 0:665a5dc5ab89 41
TetsuyaKonno 0:665a5dc5ab89 42 //LED Color on GR-PEACH
TetsuyaKonno 0:665a5dc5ab89 43 #define LED_OFF 0x00
TetsuyaKonno 0:665a5dc5ab89 44 #define LED_RED 0x01
TetsuyaKonno 0:665a5dc5ab89 45 #define LED_GREEN 0x02
TetsuyaKonno 0:665a5dc5ab89 46 #define LED_YELLOW 0x03
TetsuyaKonno 0:665a5dc5ab89 47 #define LED_BLUE 0x04
TetsuyaKonno 0:665a5dc5ab89 48 #define LED_PURPLE 0x05
TetsuyaKonno 0:665a5dc5ab89 49 #define LED_SKYBLUE 0x06
TetsuyaKonno 0:665a5dc5ab89 50 #define LED_WHITE 0x07
TetsuyaKonno 0:665a5dc5ab89 51
TetsuyaKonno 0:665a5dc5ab89 52 //Status
TetsuyaKonno 0:665a5dc5ab89 53 #define RUN 0x00
TetsuyaKonno 0:665a5dc5ab89 54 #define STOP 0x01
TetsuyaKonno 0:665a5dc5ab89 55 #define ERROR 0xff
TetsuyaKonno 0:665a5dc5ab89 56
TetsuyaKonno 0:665a5dc5ab89 57 //Constructor
TetsuyaKonno 0:665a5dc5ab89 58 //------------------------------------------------------------------//
TetsuyaKonno 0:665a5dc5ab89 59 Ticker interrput;
TetsuyaKonno 0:665a5dc5ab89 60 Serial pc(USBTX, USBRX);
TetsuyaKonno 0:665a5dc5ab89 61 DigitalOut LED_R(P6_13);
TetsuyaKonno 0:665a5dc5ab89 62 DigitalOut LED_G(P6_14);
TetsuyaKonno 0:665a5dc5ab89 63 DigitalOut LED_B(P6_15);
TetsuyaKonno 0:665a5dc5ab89 64
TetsuyaKonno 0:665a5dc5ab89 65 //Prototype
TetsuyaKonno 0:665a5dc5ab89 66 //------------------------------------------------------------------//
TetsuyaKonno 0:665a5dc5ab89 67 void init_MTU2_PWM_Servo( void ); /* Initialize PWM functions */
TetsuyaKonno 0:665a5dc5ab89 68 void intTimer( void ); /* Interrupt fanction */
TetsuyaKonno 0:665a5dc5ab89 69 void timer( unsigned long timer_set );
TetsuyaKonno 0:665a5dc5ab89 70 void handle( int angle );
TetsuyaKonno 0:665a5dc5ab89 71 void led_status_process( int set ); /* Function for only interrupt */
TetsuyaKonno 0:665a5dc5ab89 72 void led_rgb(int led);
TetsuyaKonno 0:665a5dc5ab89 73
TetsuyaKonno 0:665a5dc5ab89 74 //Globle
TetsuyaKonno 0:665a5dc5ab89 75 //------------------------------------------------------------------//
TetsuyaKonno 0:665a5dc5ab89 76 volatile int status; /* Status */
TetsuyaKonno 0:665a5dc5ab89 77 volatile unsigned int servo_offset; /* servo offset */
TetsuyaKonno 0:665a5dc5ab89 78
TetsuyaKonno 0:665a5dc5ab89 79 //Main
TetsuyaKonno 0:665a5dc5ab89 80 //------------------------------------------------------------------//
TetsuyaKonno 0:665a5dc5ab89 81 int main( void )
TetsuyaKonno 0:665a5dc5ab89 82 {
TetsuyaKonno 0:665a5dc5ab89 83 int i;
TetsuyaKonno 0:665a5dc5ab89 84 char c;
TetsuyaKonno 0:665a5dc5ab89 85
TetsuyaKonno 0:665a5dc5ab89 86 /* Initialize MCU functions */
TetsuyaKonno 0:665a5dc5ab89 87 init_MTU2_PWM_Servo();
TetsuyaKonno 0:665a5dc5ab89 88 interrput.attach(&intTimer, 0.001);
TetsuyaKonno 0:665a5dc5ab89 89 pc.baud(9600);
TetsuyaKonno 0:665a5dc5ab89 90
TetsuyaKonno 0:665a5dc5ab89 91 /* Initialize Micon Car state */
TetsuyaKonno 0:665a5dc5ab89 92 servo_offset = SERVO_CENTER;
TetsuyaKonno 0:665a5dc5ab89 93
TetsuyaKonno 0:665a5dc5ab89 94 pc.printf(
TetsuyaKonno 0:665a5dc5ab89 95 "Servo center adjustment from the PC\n\r"
TetsuyaKonno 0:665a5dc5ab89 96 "(Micon Car on GR-PEACH version)\n\r"
TetsuyaKonno 0:665a5dc5ab89 97 "\n\r"
TetsuyaKonno 0:665a5dc5ab89 98 "'Z' key : Center Value +1\n\r"
TetsuyaKonno 0:665a5dc5ab89 99 "'X' key : Center Value -1\n\r"
TetsuyaKonno 0:665a5dc5ab89 100 "\n\r"
TetsuyaKonno 0:665a5dc5ab89 101 "'A' key : Center Value +10\n\r"
TetsuyaKonno 0:665a5dc5ab89 102 "'S' key : Center Value -10\n\r"
TetsuyaKonno 0:665a5dc5ab89 103 "\n\r"
TetsuyaKonno 0:665a5dc5ab89 104 );
TetsuyaKonno 0:665a5dc5ab89 105
TetsuyaKonno 0:665a5dc5ab89 106 pc.printf( "%5d\r", servo_offset );
TetsuyaKonno 0:665a5dc5ab89 107
TetsuyaKonno 0:665a5dc5ab89 108 while( 1 ) {
TetsuyaKonno 0:665a5dc5ab89 109 status = RUN;
TetsuyaKonno 0:665a5dc5ab89 110
TetsuyaKonno 0:665a5dc5ab89 111 MTU2TGRD_0 = servo_offset;
TetsuyaKonno 0:665a5dc5ab89 112
TetsuyaKonno 0:665a5dc5ab89 113 i = pc.scanf( "%c", &c );
TetsuyaKonno 0:665a5dc5ab89 114 if( i == 1 ) {
TetsuyaKonno 0:665a5dc5ab89 115 switch( c ) {
TetsuyaKonno 0:665a5dc5ab89 116 case 'Z':
TetsuyaKonno 0:665a5dc5ab89 117 case 'z':
TetsuyaKonno 0:665a5dc5ab89 118 servo_offset += 1;
TetsuyaKonno 0:665a5dc5ab89 119 if( servo_offset > DEGREEES_MAX ) servo_offset = DEGREEES_MAX;
TetsuyaKonno 0:665a5dc5ab89 120 pc.printf( "%5d\r", servo_offset );
TetsuyaKonno 0:665a5dc5ab89 121 break;
TetsuyaKonno 0:665a5dc5ab89 122
TetsuyaKonno 0:665a5dc5ab89 123 case 'A':
TetsuyaKonno 0:665a5dc5ab89 124 case 'a':
TetsuyaKonno 0:665a5dc5ab89 125 servo_offset += 10;
TetsuyaKonno 0:665a5dc5ab89 126 if( servo_offset > DEGREEES_MAX ) servo_offset = DEGREEES_MAX;
TetsuyaKonno 0:665a5dc5ab89 127 pc.printf( "%5d\r", servo_offset );
TetsuyaKonno 0:665a5dc5ab89 128 break;
TetsuyaKonno 0:665a5dc5ab89 129
TetsuyaKonno 0:665a5dc5ab89 130 case 'X':
TetsuyaKonno 0:665a5dc5ab89 131 case 'x':
TetsuyaKonno 0:665a5dc5ab89 132 servo_offset -= 1;
TetsuyaKonno 0:665a5dc5ab89 133 if( servo_offset < DEGREEES_MIN ) servo_offset = DEGREEES_MIN;
TetsuyaKonno 0:665a5dc5ab89 134 pc.printf( "%5d\r", servo_offset );
TetsuyaKonno 0:665a5dc5ab89 135 break;
TetsuyaKonno 0:665a5dc5ab89 136
TetsuyaKonno 0:665a5dc5ab89 137 case 'S':
TetsuyaKonno 0:665a5dc5ab89 138 case 's':
TetsuyaKonno 0:665a5dc5ab89 139 servo_offset -= 10;
TetsuyaKonno 0:665a5dc5ab89 140 if( servo_offset < DEGREEES_MIN ) servo_offset = DEGREEES_MIN;
TetsuyaKonno 0:665a5dc5ab89 141 pc.printf( "%5d\r", servo_offset );
TetsuyaKonno 0:665a5dc5ab89 142 break;
TetsuyaKonno 0:665a5dc5ab89 143
TetsuyaKonno 0:665a5dc5ab89 144 default:
TetsuyaKonno 0:665a5dc5ab89 145 break;
TetsuyaKonno 0:665a5dc5ab89 146 }
TetsuyaKonno 0:665a5dc5ab89 147 }
TetsuyaKonno 0:665a5dc5ab89 148 }
TetsuyaKonno 0:665a5dc5ab89 149 }
TetsuyaKonno 0:665a5dc5ab89 150
TetsuyaKonno 0:665a5dc5ab89 151 //Initialize MTU2 PWM functions
TetsuyaKonno 0:665a5dc5ab89 152 //------------------------------------------------------------------//
TetsuyaKonno 0:665a5dc5ab89 153 //MTU2_0
TetsuyaKonno 0:665a5dc5ab89 154 //PWM mode 1
TetsuyaKonno 0:665a5dc5ab89 155 //TIOC0A(P4_0) :Servo-motor
TetsuyaKonno 0:665a5dc5ab89 156 //------------------------------------------------------------------//
TetsuyaKonno 0:665a5dc5ab89 157 void init_MTU2_PWM_Servo( void )
TetsuyaKonno 0:665a5dc5ab89 158 {
TetsuyaKonno 0:665a5dc5ab89 159 /* Port setting for S/W I/O Contorol */
TetsuyaKonno 0:665a5dc5ab89 160 /* alternative mode */
TetsuyaKonno 0:665a5dc5ab89 161
TetsuyaKonno 0:665a5dc5ab89 162 /* MTU2_0 (P4_0) */
TetsuyaKonno 0:665a5dc5ab89 163 GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/
TetsuyaKonno 0:665a5dc5ab89 164 GPIOPFCAE4 &= 0xfffe; /* The alternative function of a pin */
TetsuyaKonno 0:665a5dc5ab89 165 GPIOPFCE4 &= 0xfffe; /* The alternative function of a pin */
TetsuyaKonno 0:665a5dc5ab89 166 GPIOPFC4 |= 0x0001; /* The alternative function of a pin */
TetsuyaKonno 0:665a5dc5ab89 167 /* 2nd alternative function/output */
TetsuyaKonno 0:665a5dc5ab89 168 GPIOP4 &= 0xfffe; /* */
TetsuyaKonno 0:665a5dc5ab89 169 GPIOPM4 &= 0xfffe; /* p4_0:output */
TetsuyaKonno 0:665a5dc5ab89 170 GPIOPMC4 |= 0x0001; /* P4_0:double */
TetsuyaKonno 0:665a5dc5ab89 171
TetsuyaKonno 0:665a5dc5ab89 172 /* Mosule stop 33(MTU2) canceling */
TetsuyaKonno 0:665a5dc5ab89 173 CPGSTBCR3 &= 0xf7;
TetsuyaKonno 0:665a5dc5ab89 174
TetsuyaKonno 0:665a5dc5ab89 175 /* MTU2_0 (Motor PWM) */
TetsuyaKonno 0:665a5dc5ab89 176 MTU2TCR_0 = 0x22; /* TCNT Clear(TGRA), P0φ/16 */
TetsuyaKonno 0:665a5dc5ab89 177 MTU2TIORH_0 = 0x52; /* TGRA L>H, TGRB H>L */
TetsuyaKonno 0:665a5dc5ab89 178 MTU2TMDR_0 = 0x32; /* TGRC and TGRD = Buff-mode*/
TetsuyaKonno 0:665a5dc5ab89 179 /* PWM-mode1 */
TetsuyaKonno 0:665a5dc5ab89 180 MTU2TCNT_0 = 0; /* TCNT0 Set 0 */
TetsuyaKonno 0:665a5dc5ab89 181 MTU2TGRA_0 = MTU2TGRC_0 = SERVO_PWM_CYCLE;
TetsuyaKonno 0:665a5dc5ab89 182 /* PWM-Cycle(16ms) */
TetsuyaKonno 0:665a5dc5ab89 183 MTU2TGRB_0 = MTU2TGRD_0 = 0; /* Servo-motor(P4_0) */
TetsuyaKonno 0:665a5dc5ab89 184 MTU2TSTR |= 0x01; /* TCNT_0 Start */
TetsuyaKonno 0:665a5dc5ab89 185 }
TetsuyaKonno 0:665a5dc5ab89 186
TetsuyaKonno 0:665a5dc5ab89 187 //Interrupt Timer
TetsuyaKonno 0:665a5dc5ab89 188 //------------------------------------------------------------------//
TetsuyaKonno 0:665a5dc5ab89 189 void intTimer( void )
TetsuyaKonno 0:665a5dc5ab89 190 {
TetsuyaKonno 0:665a5dc5ab89 191 led_status_process( status );
TetsuyaKonno 0:665a5dc5ab89 192 }
TetsuyaKonno 0:665a5dc5ab89 193
TetsuyaKonno 0:665a5dc5ab89 194 //LED_Status(on GR-PEACH board) Function for only interrupt
TetsuyaKonno 0:665a5dc5ab89 195 //------------------------------------------------------------------//
TetsuyaKonno 0:665a5dc5ab89 196 void led_status_process( int set )
TetsuyaKonno 0:665a5dc5ab89 197 {
TetsuyaKonno 0:665a5dc5ab89 198 static unsigned long led_timer;
TetsuyaKonno 0:665a5dc5ab89 199 int led_set;
TetsuyaKonno 0:665a5dc5ab89 200 int on_time;
TetsuyaKonno 0:665a5dc5ab89 201 int off_time;
TetsuyaKonno 0:665a5dc5ab89 202
TetsuyaKonno 0:665a5dc5ab89 203 /* setting */
TetsuyaKonno 0:665a5dc5ab89 204 switch( set ){
TetsuyaKonno 0:665a5dc5ab89 205 case RUN:
TetsuyaKonno 0:665a5dc5ab89 206 led_set = LED_GREEN;
TetsuyaKonno 0:665a5dc5ab89 207 on_time = 500;
TetsuyaKonno 0:665a5dc5ab89 208 off_time = 500;
TetsuyaKonno 0:665a5dc5ab89 209 break;
TetsuyaKonno 0:665a5dc5ab89 210 case STOP:
TetsuyaKonno 0:665a5dc5ab89 211 led_set = LED_RED;
TetsuyaKonno 0:665a5dc5ab89 212 on_time = 500;
TetsuyaKonno 0:665a5dc5ab89 213 off_time = 0;
TetsuyaKonno 0:665a5dc5ab89 214 break;
TetsuyaKonno 0:665a5dc5ab89 215 default:
TetsuyaKonno 0:665a5dc5ab89 216 led_set = LED_OFF;
TetsuyaKonno 0:665a5dc5ab89 217 on_time = 0;
TetsuyaKonno 0:665a5dc5ab89 218 off_time = 1;
TetsuyaKonno 0:665a5dc5ab89 219 break;
TetsuyaKonno 0:665a5dc5ab89 220 }
TetsuyaKonno 0:665a5dc5ab89 221
TetsuyaKonno 0:665a5dc5ab89 222 /* Display */
TetsuyaKonno 0:665a5dc5ab89 223 led_timer++;
TetsuyaKonno 0:665a5dc5ab89 224 if( led_timer < on_time ) led_rgb( led_set );
TetsuyaKonno 0:665a5dc5ab89 225 else if( led_timer < ( on_time + off_time ) ) led_rgb( LED_OFF );
TetsuyaKonno 0:665a5dc5ab89 226 else led_timer = 0;
TetsuyaKonno 0:665a5dc5ab89 227 }
TetsuyaKonno 0:665a5dc5ab89 228
TetsuyaKonno 0:665a5dc5ab89 229 //LED_RGB(on GR-PEACH board)
TetsuyaKonno 0:665a5dc5ab89 230 //------------------------------------------------------------------//
TetsuyaKonno 0:665a5dc5ab89 231 void led_rgb(int led)
TetsuyaKonno 0:665a5dc5ab89 232 {
TetsuyaKonno 0:665a5dc5ab89 233 LED_R = led & 0x1;
TetsuyaKonno 0:665a5dc5ab89 234 LED_G = (led >> 1 ) & 0x1;
TetsuyaKonno 0:665a5dc5ab89 235 LED_B = (led >> 2 ) & 0x1;
TetsuyaKonno 0:665a5dc5ab89 236 }
TetsuyaKonno 0:665a5dc5ab89 237
TetsuyaKonno 0:665a5dc5ab89 238 //------------------------------------------------------------------//
TetsuyaKonno 0:665a5dc5ab89 239 // End of file
TetsuyaKonno 0:665a5dc5ab89 240 //------------------------------------------------------------------//