Shohei Kamiguchi / Mbed 2 deprecated Iwatobi_Control_Modulen

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@1:f52578243f96, 2020-11-15 (annotated)

Committer:
DiGengengen
Date:
Sun Nov 15 15:19:11 2020 +0000
Revision:
1:f52578243f96
Parent:
0:dde37cabc667
Child:
2:1ae5e25618e0
repaired how to receive SerialData from xbee.; not tested.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kintoki231 0:dde37cabc667 1 #include "mbed.h"
kintoki231 0:dde37cabc667 2
kintoki231 0:dde37cabc667 3 /* ピン設定 */
kintoki231 0:dde37cabc667 4 PwmOut Servo_1(PA_8);
kintoki231 0:dde37cabc667 5 // PwmOut Servo_2(PA_0); // us_tickerにてTIM2を利用中のため使用不可
kintoki231 0:dde37cabc667 6 DigitalOut Servo_2_MANUAL(PA_0);
kintoki231 0:dde37cabc667 7 PwmOut Servo_3(PA_6_ALT0);
kintoki231 0:dde37cabc667 8 PwmOut Servo_4(PA_3);
kintoki231 0:dde37cabc667 9 PwmOut Servo_5(PB_4);
kintoki231 0:dde37cabc667 10 PwmOut Servo_6(PA_7_ALT1);
kintoki231 0:dde37cabc667 11 Serial xbee(PA_9, PA_10);
kintoki231 0:dde37cabc667 12 Serial pc(USBTX, USBRX);
kintoki231 0:dde37cabc667 13 DigitalOut led[] = {
kintoki231 0:dde37cabc667 14 DigitalOut(PB_1),
kintoki231 0:dde37cabc667 15 DigitalOut(PF_0),
kintoki231 0:dde37cabc667 16 DigitalOut(PF_1),
kintoki231 0:dde37cabc667 17 };
kintoki231 0:dde37cabc667 18 DigitalOut xbee_reset(PA_1);
kintoki231 0:dde37cabc667 19 DigitalOut nucleo_led(PB_3);
kintoki231 0:dde37cabc667 20
kintoki231 0:dde37cabc667 21
kintoki231 0:dde37cabc667 22 /* モジュールID */
kintoki231 0:dde37cabc667 23 const int kModuleID = 1;
kintoki231 0:dde37cabc667 24
kintoki231 0:dde37cabc667 25
kintoki231 0:dde37cabc667 26 /* サーボ */
kintoki231 0:dde37cabc667 27 // MIN MID MAX
DiGengengen 1:f52578243f96 28 double Servo_1_pulsewidth[3] = {0.0013, 0.0015, 0.0017};
DiGengengen 1:f52578243f96 29 double Servo_2_pulsewidth[3] = {0.0013, 0.0015, 0.0017};
DiGengengen 1:f52578243f96 30 double Servo_3_pulsewidth[3] = {0.0013, 0.0015, 0.0017};
DiGengengen 1:f52578243f96 31 double Servo_4_pulsewidth[3] = {0.0013, 0.0015, 0.0017};
DiGengengen 1:f52578243f96 32 double Servo_5_pulsewidth[3] = {0.0013, 0.0015, 0.0017};
DiGengengen 1:f52578243f96 33 double Servo_6_pulsewidth[3] = {0.0013, 0.0015, 0.0017};
kintoki231 0:dde37cabc667 34
kintoki231 0:dde37cabc667 35 Ticker tick_MANUAL;
kintoki231 0:dde37cabc667 36 Timeout timo_MANUAL;
kintoki231 0:dde37cabc667 37 double Servo_2_pw_current_MANUAL = 0.001; // 適当,いじる必要なし
kintoki231 0:dde37cabc667 38 void Servo_2_off_MANUAL(){
kintoki231 0:dde37cabc667 39 Servo_2_MANUAL = 0;
kintoki231 0:dde37cabc667 40 }
kintoki231 0:dde37cabc667 41 void Servo_2_on_MANUAL(){
kintoki231 0:dde37cabc667 42 Servo_2_MANUAL = 1;
kintoki231 0:dde37cabc667 43 timo_MANUAL.attach(&Servo_2_off_MANUAL, Servo_2_pw_current_MANUAL);
kintoki231 0:dde37cabc667 44 }
kintoki231 0:dde37cabc667 45 void Servo_2_period_ms_MANUAL(int p_ms){
kintoki231 0:dde37cabc667 46 Servo_2_off_MANUAL();
kintoki231 0:dde37cabc667 47 tick_MANUAL.attach(&Servo_2_on_MANUAL, (double)p_ms / 1.0e3);
kintoki231 0:dde37cabc667 48 }
kintoki231 0:dde37cabc667 49 void Servo_2_pulsewidth_MANUAL(double p_ms){
kintoki231 0:dde37cabc667 50 Servo_2_pw_current_MANUAL = p_ms;
kintoki231 0:dde37cabc667 51 }
kintoki231 0:dde37cabc667 52
kintoki231 0:dde37cabc667 53
kintoki231 0:dde37cabc667 54 /* PM */
kintoki231 0:dde37cabc667 55 void PM_receive(){
kintoki231 0:dde37cabc667 56 while(xbee.readable()){
kintoki231 0:dde37cabc667 57 pc.putc(xbee.getc());
kintoki231 0:dde37cabc667 58 }
kintoki231 0:dde37cabc667 59 }
kintoki231 0:dde37cabc667 60 void PM_init(){
kintoki231 0:dde37cabc667 61 xbee.baud(9600);
kintoki231 0:dde37cabc667 62 xbee_reset = 1;
kintoki231 0:dde37cabc667 63 }
kintoki231 0:dde37cabc667 64 void PM_on(){
kintoki231 0:dde37cabc667 65 xbee.printf("a");
kintoki231 0:dde37cabc667 66 }
kintoki231 0:dde37cabc667 67 void PM_off(){
kintoki231 0:dde37cabc667 68 xbee.printf(" ");
kintoki231 0:dde37cabc667 69 }
kintoki231 0:dde37cabc667 70
kintoki231 0:dde37cabc667 71
kintoki231 0:dde37cabc667 72 /* PCシリアル通信 */
kintoki231 0:dde37cabc667 73 void SerialReceive(void)
kintoki231 0:dde37cabc667 74 {
kintoki231 0:dde37cabc667 75 char message = pc.getc();
kintoki231 0:dde37cabc667 76 while(pc.readable()) // bufferを綺麗に
kintoki231 0:dde37cabc667 77 pc.getc();
kintoki231 0:dde37cabc667 78
kintoki231 0:dde37cabc667 79 switch (message) {
kintoki231 0:dde37cabc667 80 case 'q':
kintoki231 0:dde37cabc667 81 Servo_1.pulsewidth(Servo_1_pulsewidth[2]);
kintoki231 0:dde37cabc667 82 break;
kintoki231 0:dde37cabc667 83 case 'a':
kintoki231 0:dde37cabc667 84 Servo_1.pulsewidth(Servo_1_pulsewidth[1]);
kintoki231 0:dde37cabc667 85 break;
kintoki231 0:dde37cabc667 86 case 'z':
kintoki231 0:dde37cabc667 87 Servo_1.pulsewidth(Servo_1_pulsewidth[0]);
kintoki231 0:dde37cabc667 88 break;
kintoki231 0:dde37cabc667 89 case 'w':
kintoki231 0:dde37cabc667 90 Servo_2_pulsewidth_MANUAL(Servo_2_pulsewidth[2]);
kintoki231 0:dde37cabc667 91 break;
kintoki231 0:dde37cabc667 92 case 's':
kintoki231 0:dde37cabc667 93 Servo_2_pulsewidth_MANUAL(Servo_2_pulsewidth[1]);
kintoki231 0:dde37cabc667 94 break;
kintoki231 0:dde37cabc667 95 case 'x':
kintoki231 0:dde37cabc667 96 Servo_2_pulsewidth_MANUAL(Servo_2_pulsewidth[0]);
kintoki231 0:dde37cabc667 97 break;
kintoki231 0:dde37cabc667 98 case 'e':
kintoki231 0:dde37cabc667 99 Servo_3.pulsewidth(Servo_3_pulsewidth[2]);
kintoki231 0:dde37cabc667 100 break;
kintoki231 0:dde37cabc667 101 case 'd':
kintoki231 0:dde37cabc667 102 Servo_3.pulsewidth(Servo_3_pulsewidth[1]);
kintoki231 0:dde37cabc667 103 break;
kintoki231 0:dde37cabc667 104 case 'c':
kintoki231 0:dde37cabc667 105 Servo_3.pulsewidth(Servo_3_pulsewidth[0]);
kintoki231 0:dde37cabc667 106 break;
kintoki231 0:dde37cabc667 107 case 'r':
kintoki231 0:dde37cabc667 108 Servo_4.pulsewidth(Servo_4_pulsewidth[2]);
kintoki231 0:dde37cabc667 109 break;
kintoki231 0:dde37cabc667 110 case 'f':
kintoki231 0:dde37cabc667 111 Servo_4.pulsewidth(Servo_4_pulsewidth[1]);
kintoki231 0:dde37cabc667 112 break;
kintoki231 0:dde37cabc667 113 case 'v':
kintoki231 0:dde37cabc667 114 Servo_4.pulsewidth(Servo_4_pulsewidth[0]);
kintoki231 0:dde37cabc667 115 break;
kintoki231 0:dde37cabc667 116 case 't':
kintoki231 0:dde37cabc667 117 Servo_5.pulsewidth(Servo_5_pulsewidth[2]);
kintoki231 0:dde37cabc667 118 break;
kintoki231 0:dde37cabc667 119 case 'g':
kintoki231 0:dde37cabc667 120 Servo_5.pulsewidth(Servo_5_pulsewidth[1]);
kintoki231 0:dde37cabc667 121 break;
kintoki231 0:dde37cabc667 122 case 'b':
kintoki231 0:dde37cabc667 123 Servo_5.pulsewidth(Servo_5_pulsewidth[0]);
kintoki231 0:dde37cabc667 124 break;
kintoki231 0:dde37cabc667 125 case 'y':
kintoki231 0:dde37cabc667 126 Servo_6.pulsewidth(Servo_6_pulsewidth[2]);
kintoki231 0:dde37cabc667 127 break;
kintoki231 0:dde37cabc667 128 case 'h':
kintoki231 0:dde37cabc667 129 Servo_6.pulsewidth(Servo_6_pulsewidth[1]);
kintoki231 0:dde37cabc667 130 break;
kintoki231 0:dde37cabc667 131 case 'n':
kintoki231 0:dde37cabc667 132 Servo_6.pulsewidth(Servo_6_pulsewidth[0]);
kintoki231 0:dde37cabc667 133 break;
kintoki231 0:dde37cabc667 134 case 'u':
kintoki231 0:dde37cabc667 135 PM_on();
kintoki231 0:dde37cabc667 136 break;
kintoki231 0:dde37cabc667 137 case 'j':
kintoki231 0:dde37cabc667 138 PM_off();
kintoki231 0:dde37cabc667 139 break;
kintoki231 0:dde37cabc667 140 default :
kintoki231 0:dde37cabc667 141 Servo_1.pulsewidth(Servo_1_pulsewidth[1]);
kintoki231 0:dde37cabc667 142 Servo_2_pulsewidth_MANUAL(Servo_2_pulsewidth[1]);
kintoki231 0:dde37cabc667 143 Servo_3.pulsewidth(Servo_3_pulsewidth[1]);
kintoki231 0:dde37cabc667 144 Servo_4.pulsewidth(Servo_4_pulsewidth[1]);
kintoki231 0:dde37cabc667 145 Servo_5.pulsewidth(Servo_5_pulsewidth[1]);
kintoki231 0:dde37cabc667 146 Servo_6.pulsewidth(Servo_6_pulsewidth[1]);
kintoki231 0:dde37cabc667 147 PM_off();
kintoki231 0:dde37cabc667 148 break;
kintoki231 0:dde37cabc667 149 }
kintoki231 0:dde37cabc667 150 led[2] =~ led[2];
kintoki231 0:dde37cabc667 151 }
kintoki231 0:dde37cabc667 152
DiGengengen 1:f52578243f96 153
DiGengengen 1:f52578243f96 154 Timer log_loop_timer;
kintoki231 0:dde37cabc667 155 int main()
kintoki231 0:dde37cabc667 156 {
kintoki231 0:dde37cabc667 157 Servo_1.period_ms(20);
kintoki231 0:dde37cabc667 158 Servo_2_period_ms_MANUAL(20);
kintoki231 0:dde37cabc667 159 Servo_3.period_ms(20);
kintoki231 0:dde37cabc667 160 Servo_4.period_ms(20);
kintoki231 0:dde37cabc667 161 Servo_5.period_ms(20);
kintoki231 0:dde37cabc667 162 Servo_6.period_ms(20);
kintoki231 0:dde37cabc667 163 Servo_1.pulsewidth(Servo_1_pulsewidth[1]);
kintoki231 0:dde37cabc667 164 Servo_2_pulsewidth_MANUAL(Servo_2_pulsewidth[1]);
kintoki231 0:dde37cabc667 165 Servo_3.pulsewidth(Servo_3_pulsewidth[1]);
kintoki231 0:dde37cabc667 166 Servo_4.pulsewidth(Servo_4_pulsewidth[1]);
kintoki231 0:dde37cabc667 167 Servo_5.pulsewidth(Servo_5_pulsewidth[1]);
kintoki231 0:dde37cabc667 168 Servo_6.pulsewidth(Servo_6_pulsewidth[1]);
kintoki231 0:dde37cabc667 169 PM_off();
kintoki231 0:dde37cabc667 170
kintoki231 0:dde37cabc667 171 pc.baud(9600);
kintoki231 0:dde37cabc667 172 pc.attach(SerialReceive,Serial::RxIrq);
kintoki231 0:dde37cabc667 173 PM_init();
kintoki231 0:dde37cabc667 174
kintoki231 0:dde37cabc667 175 // start motion
kintoki231 0:dde37cabc667 176 wait(1);
kintoki231 0:dde37cabc667 177
kintoki231 0:dde37cabc667 178 pc.printf("CM : %02d\n", kModuleID);
kintoki231 0:dde37cabc667 179
kintoki231 0:dde37cabc667 180 wait(1);
kintoki231 0:dde37cabc667 181 led[0] = 1;
DiGengengen 1:f52578243f96 182
kintoki231 0:dde37cabc667 183 nucleo_led = 0;
kintoki231 0:dde37cabc667 184 while(1) {
DiGengengen 1:f52578243f96 185 log_loop_timer.reset();
DiGengengen 1:f52578243f96 186 log_loop_timer.start();
DiGengengen 1:f52578243f96 187
kintoki231 0:dde37cabc667 188 pc.printf("CM : %02d\n", kModuleID);
DiGengengen 1:f52578243f96 189
DiGengengen 1:f52578243f96 190 while(log_loop_timer.read() < 0.5f){
DiGengengen 1:f52578243f96 191 PM_receive();
DiGengengen 1:f52578243f96 192 }
DiGengengen 1:f52578243f96 193
kintoki231 0:dde37cabc667 194 nucleo_led =~ nucleo_led;
DiGengengen 1:f52578243f96 195 log_loop_timer.stop();
kintoki231 0:dde37cabc667 196 }
kintoki231 0:dde37cabc667 197 }