Robotics Term Project / Mbed 2 deprecated Nucleo_servo_bluetooth

123

Dependencies:   mbed

main.cpp@0:7ec5279f429e, 2016-05-24 (annotated)

Committer:
cpul5338
Date:
Tue May 24 05:16:21 2016 +0000
Revision:
0:7ec5279f429e
123;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cpul5338 0:7ec5279f429e 1 /*LAB_SERVO*/
cpul5338 0:7ec5279f429e 2 #include "mbed.h"
cpul5338 0:7ec5279f429e 3
cpul5338 0:7ec5279f429e 4 PwmOut servo(A0);
cpul5338 0:7ec5279f429e 5 Serial bluetooth(D10,D2);
cpul5338 0:7ec5279f429e 6 Serial pc(D1,D0);
cpul5338 0:7ec5279f429e 7
cpul5338 0:7ec5279f429e 8 //LED1 = D13 = PA_5 (LED on Nucleo board)
cpul5338 0:7ec5279f429e 9 DigitalOut led1(A4);
cpul5338 0:7ec5279f429e 10 DigitalOut led2(A5);
cpul5338 0:7ec5279f429e 11 int counter;
cpul5338 0:7ec5279f429e 12 int angle = 0;
cpul5338 0:7ec5279f429e 13
cpul5338 0:7ec5279f429e 14 Ticker timer1;
cpul5338 0:7ec5279f429e 15 void timer1_interrupt(void);
cpul5338 0:7ec5279f429e 16
cpul5338 0:7ec5279f429e 17 void init_TIMER(void);
cpul5338 0:7ec5279f429e 18 void init_IO(void);
cpul5338 0:7ec5279f429e 19 void flash(void);
cpul5338 0:7ec5279f429e 20
cpul5338 0:7ec5279f429e 21 //Variable(s) for internal control
cpul5338 0:7ec5279f429e 22 float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90
cpul5338 0:7ec5279f429e 23
cpul5338 0:7ec5279f429e 24
cpul5338 0:7ec5279f429e 25 int main (void)
cpul5338 0:7ec5279f429e 26 {
cpul5338 0:7ec5279f429e 27 init_IO();
cpul5338 0:7ec5279f429e 28 init_TIMER();
cpul5338 0:7ec5279f429e 29 bluetooth.baud(115200); //設定鮑率
cpul5338 0:7ec5279f429e 30 pc.baud(57600);
cpul5338 0:7ec5279f429e 31
cpul5338 0:7ec5279f429e 32 while(1)
cpul5338 0:7ec5279f429e 33 {
cpul5338 0:7ec5279f429e 34 if(pc.readable())
cpul5338 0:7ec5279f429e 35 {
cpul5338 0:7ec5279f429e 36 bluetooth.putc(pc.getc());
cpul5338 0:7ec5279f429e 37 }
cpul5338 0:7ec5279f429e 38 if(bluetooth.readable())
cpul5338 0:7ec5279f429e 39 {
cpul5338 0:7ec5279f429e 40 pc.putc(bluetooth.getc());
cpul5338 0:7ec5279f429e 41 }
cpul5338 0:7ec5279f429e 42 }
cpul5338 0:7ec5279f429e 43 }
cpul5338 0:7ec5279f429e 44
cpul5338 0:7ec5279f429e 45 void timer1_interrupt(void)
cpul5338 0:7ec5279f429e 46 {
cpul5338 0:7ec5279f429e 47 counter++;
cpul5338 0:7ec5279f429e 48 if(counter == 100)
cpul5338 0:7ec5279f429e 49 {
cpul5338 0:7ec5279f429e 50 led1 = 1;
cpul5338 0:7ec5279f429e 51 wait(1);
cpul5338 0:7ec5279f429e 52 led1 = 0;
cpul5338 0:7ec5279f429e 53 }
cpul5338 0:7ec5279f429e 54 else if(counter == 200)
cpul5338 0:7ec5279f429e 55 {
cpul5338 0:7ec5279f429e 56 led2 = 1;
cpul5338 0:7ec5279f429e 57 counter = 0;
cpul5338 0:7ec5279f429e 58 wait(1);
cpul5338 0:7ec5279f429e 59 led2 = 0;
cpul5338 0:7ec5279f429e 60 }
cpul5338 0:7ec5279f429e 61
cpul5338 0:7ec5279f429e 62 //////code for internal control//////
cpul5338 0:7ec5279f429e 63
cpul5338 0:7ec5279f429e 64 switch(bluetooth.getc()){
cpul5338 0:7ec5279f429e 65 case 'a':
cpul5338 0:7ec5279f429e 66 angle = 30;
cpul5338 0:7ec5279f429e 67 break;
cpul5338 0:7ec5279f429e 68 case 'b':
cpul5338 0:7ec5279f429e 69 angle = 60;
cpul5338 0:7ec5279f429e 70 break;
cpul5338 0:7ec5279f429e 71 case 'c':
cpul5338 0:7ec5279f429e 72 angle = 90;
cpul5338 0:7ec5279f429e 73 break;
cpul5338 0:7ec5279f429e 74 case 'd':
cpul5338 0:7ec5279f429e 75 angle = 0;
cpul5338 0:7ec5279f429e 76 break;
cpul5338 0:7ec5279f429e 77
cpul5338 0:7ec5279f429e 78 }
cpul5338 0:7ec5279f429e 79
cpul5338 0:7ec5279f429e 80
cpul5338 0:7ec5279f429e 81
cpul5338 0:7ec5279f429e 82 servo_duty = 0.079 + (0.084/180)*angle;
cpul5338 0:7ec5279f429e 83 servo.write(servo_duty);
cpul5338 0:7ec5279f429e 84 servo = 1;
cpul5338 0:7ec5279f429e 85 wait(0.1);
cpul5338 0:7ec5279f429e 86 servo = 0;
cpul5338 0:7ec5279f429e 87
cpul5338 0:7ec5279f429e 88 ////////////////////////////////////////////
cpul5338 0:7ec5279f429e 89
cpul5338 0:7ec5279f429e 90 if(servo_duty >= 0.121f)servo_duty = 0.121;
cpul5338 0:7ec5279f429e 91 else if(servo_duty <= 0.037f)servo_duty = 0.037;
cpul5338 0:7ec5279f429e 92 servo.write(servo_duty);
cpul5338 0:7ec5279f429e 93 }
cpul5338 0:7ec5279f429e 94
cpul5338 0:7ec5279f429e 95 void init_TIMER(void)
cpul5338 0:7ec5279f429e 96 {
cpul5338 0:7ec5279f429e 97 timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
cpul5338 0:7ec5279f429e 98 }
cpul5338 0:7ec5279f429e 99
cpul5338 0:7ec5279f429e 100 void init_IO(void)
cpul5338 0:7ec5279f429e 101 {
cpul5338 0:7ec5279f429e 102 counter = 0;
cpul5338 0:7ec5279f429e 103 led1 = 0;
cpul5338 0:7ec5279f429e 104 led2 = 0;
cpul5338 0:7ec5279f429e 105 }
cpul5338 0:7ec5279f429e 106
cpul5338 0:7ec5279f429e 107 void flash(void)
cpul5338 0:7ec5279f429e 108 {
cpul5338 0:7ec5279f429e 109 led1 = !led1;
cpul5338 0:7ec5279f429e 110 }