Robotics Term Project / Mbed 2 deprecated Robotics_Lab_BLEServo

Robotics_Lab_BLEServo

Dependencies:   mbed

main.cpp@0:d51a060965e9, 2016-05-25 (annotated)

Committer:
roger5641
Date:
Wed May 25 07:01:38 2016 +0000
Revision:
0:d51a060965e9
BLE_Servo

Who changed what in which revision?

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