シリアルモニタに @角度<CR><LF> と入力すると、その角度にサーボが回るプログラムです。
Hybrid_ServoMotor.cpp@0:2d7d98ccddfa, 2017-01-18 (annotated)
- Committer:
- Gaku0606
- Date:
- Wed Jan 18 22:12:15 2017 +0000
- Revision:
- 0:2d7d98ccddfa
????????@??<CR><LF> ??????????????????????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Gaku0606 | 0:2d7d98ccddfa | 1 | #include "mbed.h" |
Gaku0606 | 0:2d7d98ccddfa | 2 | #include "Servo.h" |
Gaku0606 | 0:2d7d98ccddfa | 3 | |
Gaku0606 | 0:2d7d98ccddfa | 4 | RawSerial pc(USBTX, USBRX); |
Gaku0606 | 0:2d7d98ccddfa | 5 | |
Gaku0606 | 0:2d7d98ccddfa | 6 | #define SERVO1_PIN p22 |
Gaku0606 | 0:2d7d98ccddfa | 7 | |
Gaku0606 | 0:2d7d98ccddfa | 8 | #define MAX_PULSE 0.00245//[s] |
Gaku0606 | 0:2d7d98ccddfa | 9 | #define MIN_PULSE 0.00065//[s] |
Gaku0606 | 0:2d7d98ccddfa | 10 | Servo servo1(SERVO1_PIN, MAX_PULSE, MIN_PULSE); |
Gaku0606 | 0:2d7d98ccddfa | 11 | |
Gaku0606 | 0:2d7d98ccddfa | 12 | DigitalOut led1(LED1); |
Gaku0606 | 0:2d7d98ccddfa | 13 | DigitalOut led2(LED2); |
Gaku0606 | 0:2d7d98ccddfa | 14 | DigitalOut led3(LED3); |
Gaku0606 | 0:2d7d98ccddfa | 15 | DigitalOut led4(LED4); |
Gaku0606 | 0:2d7d98ccddfa | 16 | |
Gaku0606 | 0:2d7d98ccddfa | 17 | DigitalOut nicrom(p21); |
Gaku0606 | 0:2d7d98ccddfa | 18 | |
Gaku0606 | 0:2d7d98ccddfa | 19 | void angleChange(){ |
Gaku0606 | 0:2d7d98ccddfa | 20 | const int BUFFER_SIZE = 16; |
Gaku0606 | 0:2d7d98ccddfa | 21 | static float angle = 0; |
Gaku0606 | 0:2d7d98ccddfa | 22 | static char buff[BUFFER_SIZE] = {'\0'}; |
Gaku0606 | 0:2d7d98ccddfa | 23 | static int count = 0; |
Gaku0606 | 0:2d7d98ccddfa | 24 | static bool startFlag = false; |
Gaku0606 | 0:2d7d98ccddfa | 25 | |
Gaku0606 | 0:2d7d98ccddfa | 26 | char temp = pc.getc(); |
Gaku0606 | 0:2d7d98ccddfa | 27 | if(temp == 'u') nicrom = 1; |
Gaku0606 | 0:2d7d98ccddfa | 28 | else if(temp == 'd') nicrom = 0; |
Gaku0606 | 0:2d7d98ccddfa | 29 | pc.printf("%c", temp); |
Gaku0606 | 0:2d7d98ccddfa | 30 | if(temp == '@'){ |
Gaku0606 | 0:2d7d98ccddfa | 31 | startFlag = true; |
Gaku0606 | 0:2d7d98ccddfa | 32 | memset(buff, '\0', BUFFER_SIZE); |
Gaku0606 | 0:2d7d98ccddfa | 33 | |
Gaku0606 | 0:2d7d98ccddfa | 34 | return; |
Gaku0606 | 0:2d7d98ccddfa | 35 | } |
Gaku0606 | 0:2d7d98ccddfa | 36 | if(startFlag){ |
Gaku0606 | 0:2d7d98ccddfa | 37 | |
Gaku0606 | 0:2d7d98ccddfa | 38 | if(temp == '\r'){ |
Gaku0606 | 0:2d7d98ccddfa | 39 | buff[count] = '\0'; |
Gaku0606 | 0:2d7d98ccddfa | 40 | startFlag = false; |
Gaku0606 | 0:2d7d98ccddfa | 41 | angle = (float)atof(buff); |
Gaku0606 | 0:2d7d98ccddfa | 42 | servo1 = angle; |
Gaku0606 | 0:2d7d98ccddfa | 43 | pc.printf("\r\nangle : %3.2f [degree]\r\n", servo1()); |
Gaku0606 | 0:2d7d98ccddfa | 44 | pc.printf("Please send like @***.*<CR><LF>\r\n\r\n"); |
Gaku0606 | 0:2d7d98ccddfa | 45 | pc.printf("---------------------------------------\r\n"); |
Gaku0606 | 0:2d7d98ccddfa | 46 | /* |
Gaku0606 | 0:2d7d98ccddfa | 47 | if(angle < -90.0) angle = -90.0f; |
Gaku0606 | 0:2d7d98ccddfa | 48 | else if(angle > 90.0) angle = 90.0f; |
Gaku0606 | 0:2d7d98ccddfa | 49 | servo1.position(angle); |
Gaku0606 | 0:2d7d98ccddfa | 50 | pc.printf("angle : %3.0f [degree]\r\n", angle);*/ |
Gaku0606 | 0:2d7d98ccddfa | 51 | |
Gaku0606 | 0:2d7d98ccddfa | 52 | memset(buff, '\0', BUFFER_SIZE); |
Gaku0606 | 0:2d7d98ccddfa | 53 | count = 0; |
Gaku0606 | 0:2d7d98ccddfa | 54 | return; |
Gaku0606 | 0:2d7d98ccddfa | 55 | } |
Gaku0606 | 0:2d7d98ccddfa | 56 | buff[count] = temp; |
Gaku0606 | 0:2d7d98ccddfa | 57 | count++; |
Gaku0606 | 0:2d7d98ccddfa | 58 | if(count > BUFFER_SIZE - 1){//null文字分空けておく |
Gaku0606 | 0:2d7d98ccddfa | 59 | startFlag = false; |
Gaku0606 | 0:2d7d98ccddfa | 60 | memset(buff, '\0', BUFFER_SIZE); |
Gaku0606 | 0:2d7d98ccddfa | 61 | count = 0; |
Gaku0606 | 0:2d7d98ccddfa | 62 | pc.printf("\r"); |
Gaku0606 | 0:2d7d98ccddfa | 63 | pc.printf("\033[K"); |
Gaku0606 | 0:2d7d98ccddfa | 64 | return; |
Gaku0606 | 0:2d7d98ccddfa | 65 | } |
Gaku0606 | 0:2d7d98ccddfa | 66 | |
Gaku0606 | 0:2d7d98ccddfa | 67 | } |
Gaku0606 | 0:2d7d98ccddfa | 68 | } |
Gaku0606 | 0:2d7d98ccddfa | 69 | |
Gaku0606 | 0:2d7d98ccddfa | 70 | void ledDriver(unsigned char bin){ |
Gaku0606 | 0:2d7d98ccddfa | 71 | unsigned char cmd = bin & 0b00000001; |
Gaku0606 | 0:2d7d98ccddfa | 72 | if(cmd) led1 = 1; |
Gaku0606 | 0:2d7d98ccddfa | 73 | else led1 = 0; |
Gaku0606 | 0:2d7d98ccddfa | 74 | cmd = bin & 0b00000010; |
Gaku0606 | 0:2d7d98ccddfa | 75 | if(cmd) led2 = 1; |
Gaku0606 | 0:2d7d98ccddfa | 76 | else led2 = 0; |
Gaku0606 | 0:2d7d98ccddfa | 77 | cmd = bin & 0b00000100; |
Gaku0606 | 0:2d7d98ccddfa | 78 | if(cmd) led3 = 1; |
Gaku0606 | 0:2d7d98ccddfa | 79 | else led3 = 0; |
Gaku0606 | 0:2d7d98ccddfa | 80 | cmd = bin & 0b00001000; |
Gaku0606 | 0:2d7d98ccddfa | 81 | if(cmd) led4 = 1; |
Gaku0606 | 0:2d7d98ccddfa | 82 | else led4 = 0; |
Gaku0606 | 0:2d7d98ccddfa | 83 | } |
Gaku0606 | 0:2d7d98ccddfa | 84 | |
Gaku0606 | 0:2d7d98ccddfa | 85 | int main() { |
Gaku0606 | 0:2d7d98ccddfa | 86 | |
Gaku0606 | 0:2d7d98ccddfa | 87 | wait(3.0); |
Gaku0606 | 0:2d7d98ccddfa | 88 | |
Gaku0606 | 0:2d7d98ccddfa | 89 | pc.baud(115200); |
Gaku0606 | 0:2d7d98ccddfa | 90 | pc.attach(&angleChange, Serial::RxIrq); |
Gaku0606 | 0:2d7d98ccddfa | 91 | //char count = 0b00000001; |
Gaku0606 | 0:2d7d98ccddfa | 92 | pc.printf("\r\nStart boot phase:\r\n"); |
Gaku0606 | 0:2d7d98ccddfa | 93 | for(int i = 0; i < 100; i++){ |
Gaku0606 | 0:2d7d98ccddfa | 94 | pc.printf("\033[K"); |
Gaku0606 | 0:2d7d98ccddfa | 95 | pc.printf("Loading... : %3d [%%]\r", i); |
Gaku0606 | 0:2d7d98ccddfa | 96 | wait(0.01); |
Gaku0606 | 0:2d7d98ccddfa | 97 | } |
Gaku0606 | 0:2d7d98ccddfa | 98 | pc.printf("Loading... : %3d [%%]\r\n", 100); |
Gaku0606 | 0:2d7d98ccddfa | 99 | pc.printf(" -> OK!\r\n"); |
Gaku0606 | 0:2d7d98ccddfa | 100 | wait(1.0); |
Gaku0606 | 0:2d7d98ccddfa | 101 | pc.printf("Initialize...\r\n"); |
Gaku0606 | 0:2d7d98ccddfa | 102 | for(int i = 0;i < 20; i++){ |
Gaku0606 | 0:2d7d98ccddfa | 103 | ledDriver(0b00001111); |
Gaku0606 | 0:2d7d98ccddfa | 104 | wait(0.05); |
Gaku0606 | 0:2d7d98ccddfa | 105 | ledDriver(0b00000000); |
Gaku0606 | 0:2d7d98ccddfa | 106 | wait(0.05); |
Gaku0606 | 0:2d7d98ccddfa | 107 | } |
Gaku0606 | 0:2d7d98ccddfa | 108 | servo1 = 0.0f; |
Gaku0606 | 0:2d7d98ccddfa | 109 | pc.printf("angle : %3.2f [degree]\r\n", servo1()); |
Gaku0606 | 0:2d7d98ccddfa | 110 | pc.printf(" -> OK!\r\n"); |
Gaku0606 | 0:2d7d98ccddfa | 111 | wait(0.5); |
Gaku0606 | 0:2d7d98ccddfa | 112 | |
Gaku0606 | 0:2d7d98ccddfa | 113 | pc.printf("End boot phase\r\n"); |
Gaku0606 | 0:2d7d98ccddfa | 114 | wait(1.0); |
Gaku0606 | 0:2d7d98ccddfa | 115 | pc.printf("Start _Servo Driving System ver 1.5.1_\r\n"); |
Gaku0606 | 0:2d7d98ccddfa | 116 | pc.printf("\r\nPlease send like @***.*<CR><LF>, ***.* = angle[degree]\r\n"); |
Gaku0606 | 0:2d7d98ccddfa | 117 | |
Gaku0606 | 0:2d7d98ccddfa | 118 | while(1) { |
Gaku0606 | 0:2d7d98ccddfa | 119 | ledDriver(0b00000001); |
Gaku0606 | 0:2d7d98ccddfa | 120 | wait(0.2); |
Gaku0606 | 0:2d7d98ccddfa | 121 | ledDriver(0b00000010); |
Gaku0606 | 0:2d7d98ccddfa | 122 | wait(0.2); |
Gaku0606 | 0:2d7d98ccddfa | 123 | ledDriver(0b00000100); |
Gaku0606 | 0:2d7d98ccddfa | 124 | wait(0.2); |
Gaku0606 | 0:2d7d98ccddfa | 125 | ledDriver(0b00001000); |
Gaku0606 | 0:2d7d98ccddfa | 126 | wait(0.2); |
Gaku0606 | 0:2d7d98ccddfa | 127 | } |
Gaku0606 | 0:2d7d98ccddfa | 128 | } |