PQ_Hybrid_Electrical_Equipment_Team / Hybrid_ServoMotor

シリアルモニタに @角度<CR><LF> と入力すると、その角度にサーボが回るプログラムです。

Dependencies:   Servo mbed-dev

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?

UserRevisionLine numberNew 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 }