Takeuchi Kouichi / Mbed 2 deprecated 20130530_Tamagawa_IMU_3Servo

多摩川精機製IMUを接続し、3サーボを制御するモーショントラッキングシステム用プログラムです。

Dependencies:   mbed

main.cpp@1:a4ab99c0f7ea, 2013-05-29 (annotated)

Committer:
takeuchi
Date:
Wed May 29 06:45:00 2013 +0000
Revision:
1:a4ab99c0f7ea
Parent:
0:0522a96f04ed 
Tamagawa IMU 3servo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
takeuchi 1:a4ab99c0f7ea 1 // Tamagawaseiki IMU Demo02
takeuchi 1:a4ab99c0f7ea 2 // 3Servo Demonstration
takeuchi 1:a4ab99c0f7ea 3 #include "mbed.h"
takeuchi 1:a4ab99c0f7ea 4 #include "TextLCD0420.h"
takeuchi 1:a4ab99c0f7ea 5
takeuchi 1:a4ab99c0f7ea 6 #define ON 1
takeuchi 1:a4ab99c0f7ea 7 #define OFF 0
takeuchi 1:a4ab99c0f7ea 8
takeuchi 1:a4ab99c0f7ea 9 DigitalOut mled0(LED1);
takeuchi 1:a4ab99c0f7ea 10 DigitalOut mled1(LED2);
takeuchi 1:a4ab99c0f7ea 11 PwmOut servo1(p21);
takeuchi 1:a4ab99c0f7ea 12 PwmOut servo2(p22);
takeuchi 1:a4ab99c0f7ea 13 PwmOut servo3(p23);
takeuchi 1:a4ab99c0f7ea 14
takeuchi 1:a4ab99c0f7ea 15 TextLCD lcd(p24, p25, p26, p27, p28, p29, p30,20,4); // rs, rw, e, d0, d1, d2, d3
takeuchi 1:a4ab99c0f7ea 16 Serial pc(USBTX, USBRX); // tx, rx
takeuchi 1:a4ab99c0f7ea 17 Serial IMU(p9,p10);// p9=tx,p10=rx
takeuchi 1:a4ab99c0f7ea 18
takeuchi 1:a4ab99c0f7ea 19 double rx,px,ax;
takeuchi 1:a4ab99c0f7ea 20 double rx_imu,px_imu,ax_imu;
takeuchi 1:a4ab99c0f7ea 21
takeuchi 1:a4ab99c0f7ea 22 void IMU_get(){
takeuchi 1:a4ab99c0f7ea 23
takeuchi 1:a4ab99c0f7ea 24 unsigned char c1,c2;
takeuchi 1:a4ab99c0f7ea 25 int i;
takeuchi 1:a4ab99c0f7ea 26
takeuchi 1:a4ab99c0f7ea 27 while(IMU.getc()!='$'){
takeuchi 1:a4ab99c0f7ea 28 }
takeuchi 1:a4ab99c0f7ea 29 for(i=1;i<=8;i++){
takeuchi 1:a4ab99c0f7ea 30 c1=IMU.getc();
takeuchi 1:a4ab99c0f7ea 31 }
takeuchi 1:a4ab99c0f7ea 32 for(i=1;i<=20;i++){
takeuchi 1:a4ab99c0f7ea 33 c1=IMU.getc();
takeuchi 1:a4ab99c0f7ea 34 }
takeuchi 1:a4ab99c0f7ea 35 c1=IMU.getc();
takeuchi 1:a4ab99c0f7ea 36 c2=IMU.getc();
takeuchi 1:a4ab99c0f7ea 37 rx_imu=float((c1 << 8) | c2)*180/32768;
takeuchi 1:a4ab99c0f7ea 38
takeuchi 1:a4ab99c0f7ea 39 c1=IMU.getc();
takeuchi 1:a4ab99c0f7ea 40 c2=IMU.getc();
takeuchi 1:a4ab99c0f7ea 41 px_imu=float((c1 << 8) | c2)*180/32768;
takeuchi 1:a4ab99c0f7ea 42
takeuchi 1:a4ab99c0f7ea 43 c1=IMU.getc();
takeuchi 1:a4ab99c0f7ea 44 c2=IMU.getc();
takeuchi 1:a4ab99c0f7ea 45 ax=float((c1 << 8) | c2)*180/32768;
takeuchi 1:a4ab99c0f7ea 46
takeuchi 1:a4ab99c0f7ea 47 //for rx
takeuchi 1:a4ab99c0f7ea 48 if( rx_imu >= 0 && rx_imu <= 90){
takeuchi 1:a4ab99c0f7ea 49 rx=rx_imu+90;
takeuchi 1:a4ab99c0f7ea 50 }
takeuchi 1:a4ab99c0f7ea 51 else if( rx > 90 && rx < 180){
takeuchi 1:a4ab99c0f7ea 52 //rx=180;
takeuchi 1:a4ab99c0f7ea 53 }
takeuchi 1:a4ab99c0f7ea 54 else if( rx >= 180 && rx < 270){
takeuchi 1:a4ab99c0f7ea 55 //rx=0;
takeuchi 1:a4ab99c0f7ea 56 }
takeuchi 1:a4ab99c0f7ea 57 else if(rx_imu >= 270 && rx_imu <= 360){
takeuchi 1:a4ab99c0f7ea 58 rx=rx_imu-270;
takeuchi 1:a4ab99c0f7ea 59 }
takeuchi 1:a4ab99c0f7ea 60
takeuchi 1:a4ab99c0f7ea 61 //for px
takeuchi 1:a4ab99c0f7ea 62 if( px_imu >= 0 && px_imu <= 90){
takeuchi 1:a4ab99c0f7ea 63 px=px_imu+90;
takeuchi 1:a4ab99c0f7ea 64 }
takeuchi 1:a4ab99c0f7ea 65 else if( px > 90 && px < 180){
takeuchi 1:a4ab99c0f7ea 66 //px=180;
takeuchi 1:a4ab99c0f7ea 67 }
takeuchi 1:a4ab99c0f7ea 68 else if(px >=180 && px < 270){
takeuchi 1:a4ab99c0f7ea 69 //px=0;
takeuchi 1:a4ab99c0f7ea 70 }
takeuchi 1:a4ab99c0f7ea 71 else if(px_imu >= 270 && px_imu <= 360){
takeuchi 1:a4ab99c0f7ea 72 px=px_imu-270;
takeuchi 1:a4ab99c0f7ea 73 }
takeuchi 1:a4ab99c0f7ea 74
takeuchi 1:a4ab99c0f7ea 75 // for ax
takeuchi 1:a4ab99c0f7ea 76 if(ax >= 0 && ax <=90){
takeuchi 1:a4ab99c0f7ea 77 ax=ax+90;
takeuchi 1:a4ab99c0f7ea 78 }
takeuchi 1:a4ab99c0f7ea 79 else if(ax >90 && ax <180){
takeuchi 1:a4ab99c0f7ea 80 ax=180;
takeuchi 1:a4ab99c0f7ea 81 }
takeuchi 1:a4ab99c0f7ea 82 else if(ax >= 180 && ax <270){
takeuchi 1:a4ab99c0f7ea 83 ax=0;
takeuchi 1:a4ab99c0f7ea 84 }
takeuchi 1:a4ab99c0f7ea 85 else if(ax >= 270 && ax <=360){
takeuchi 1:a4ab99c0f7ea 86 ax=ax-270;
takeuchi 1:a4ab99c0f7ea 87 }
takeuchi 1:a4ab99c0f7ea 88
takeuchi 1:a4ab99c0f7ea 89 }
takeuchi 1:a4ab99c0f7ea 90
takeuchi 1:a4ab99c0f7ea 91 int main() {
takeuchi 1:a4ab99c0f7ea 92
takeuchi 1:a4ab99c0f7ea 93 int i,j,k;
takeuchi 1:a4ab99c0f7ea 94 int rx_lc=9,rx_lc_old=9;
takeuchi 1:a4ab99c0f7ea 95 int px_lc=9,px_lc_old=9;
takeuchi 1:a4ab99c0f7ea 96 int ax_lc=9,ax_lc_old=9;
takeuchi 1:a4ab99c0f7ea 97 double rx_pwidth,px_pwidth,ax_pwidth;
takeuchi 1:a4ab99c0f7ea 98
takeuchi 1:a4ab99c0f7ea 99 servo1.period_ms(20);
takeuchi 1:a4ab99c0f7ea 100 servo2.period_ms(20);
takeuchi 1:a4ab99c0f7ea 101 servo3.period_ms(20);
takeuchi 1:a4ab99c0f7ea 102
takeuchi 1:a4ab99c0f7ea 103 //IMU.baud(57600);
takeuchi 1:a4ab99c0f7ea 104 IMU.baud(119200);
takeuchi 1:a4ab99c0f7ea 105 lcd.cls();
takeuchi 1:a4ab99c0f7ea 106 lcd.locate(0,0);
takeuchi 1:a4ab99c0f7ea 107 lcd.printf("Tamagawaseiki IMU\n");
takeuchi 1:a4ab99c0f7ea 108 lcd.printf("3Servo Demo Ver.02\n");
takeuchi 1:a4ab99c0f7ea 109 lcd.printf("System start ");
takeuchi 1:a4ab99c0f7ea 110
takeuchi 1:a4ab99c0f7ea 111 servo1.pulsewidth(1.5/1000);
takeuchi 1:a4ab99c0f7ea 112 servo2.pulsewidth(1.5/1000);
takeuchi 1:a4ab99c0f7ea 113 servo3.pulsewidth(1.5/1000);
takeuchi 1:a4ab99c0f7ea 114
takeuchi 1:a4ab99c0f7ea 115 //IMU.printf("$TSC,OFC,10*CC\r\n");
takeuchi 1:a4ab99c0f7ea 116
takeuchi 1:a4ab99c0f7ea 117 IMU.printf("$TSC,RAW,50\r\n");// IMU data output start
takeuchi 1:a4ab99c0f7ea 118 for(i=0;i<5;i++){
takeuchi 1:a4ab99c0f7ea 119 lcd.printf(".");
takeuchi 1:a4ab99c0f7ea 120 IMU.printf("$TSC,RAW,50\r\n");
takeuchi 1:a4ab99c0f7ea 121 wait(1.0);
takeuchi 1:a4ab99c0f7ea 122 }
takeuchi 1:a4ab99c0f7ea 123
takeuchi 1:a4ab99c0f7ea 124 IMU.printf("$TSC,HRST*75\r\n");// ax reset
takeuchi 1:a4ab99c0f7ea 125
takeuchi 1:a4ab99c0f7ea 126 IMU_get();
takeuchi 1:a4ab99c0f7ea 127 lcd.cls();
takeuchi 1:a4ab99c0f7ea 128 wait(0.1);
takeuchi 1:a4ab99c0f7ea 129
takeuchi 1:a4ab99c0f7ea 130 while (1) {
takeuchi 1:a4ab99c0f7ea 131 IMU_get();
takeuchi 1:a4ab99c0f7ea 132
takeuchi 1:a4ab99c0f7ea 133 //for rx
takeuchi 1:a4ab99c0f7ea 134 if( rx >= 88 && rx <= 92){
takeuchi 1:a4ab99c0f7ea 135 rx=90;
takeuchi 1:a4ab99c0f7ea 136 }
takeuchi 1:a4ab99c0f7ea 137 if( rx >= 170 && rx < 180){
takeuchi 1:a4ab99c0f7ea 138 rx=170;
takeuchi 1:a4ab99c0f7ea 139 }
takeuchi 1:a4ab99c0f7ea 140 else if( rx <=10 && rx > 0){
takeuchi 1:a4ab99c0f7ea 141 rx=10;
takeuchi 1:a4ab99c0f7ea 142 }
takeuchi 1:a4ab99c0f7ea 143
takeuchi 1:a4ab99c0f7ea 144 //for px
takeuchi 1:a4ab99c0f7ea 145 if( px >= 88 && px <= 92){
takeuchi 1:a4ab99c0f7ea 146 px=90;
takeuchi 1:a4ab99c0f7ea 147 }
takeuchi 1:a4ab99c0f7ea 148 if( px >= 130 ){
takeuchi 1:a4ab99c0f7ea 149 px=130;
takeuchi 1:a4ab99c0f7ea 150 }
takeuchi 1:a4ab99c0f7ea 151 else if(px < 50 ){
takeuchi 1:a4ab99c0f7ea 152 px=50;
takeuchi 1:a4ab99c0f7ea 153 }
takeuchi 1:a4ab99c0f7ea 154
takeuchi 1:a4ab99c0f7ea 155 lcd.locate(0,0);
takeuchi 1:a4ab99c0f7ea 156 lcd.printf("%5.1f,%5.1f,%6.2f\n",rx,px,ax);
takeuchi 1:a4ab99c0f7ea 157
takeuchi 1:a4ab99c0f7ea 158 rx_lc=rx/10;
takeuchi 1:a4ab99c0f7ea 159 px_lc=px/10;
takeuchi 1:a4ab99c0f7ea 160 ax_lc=ax/10;
takeuchi 1:a4ab99c0f7ea 161
takeuchi 1:a4ab99c0f7ea 162 ax=-ax+180;//servo pulse reverse
takeuchi 1:a4ab99c0f7ea 163
takeuchi 1:a4ab99c0f7ea 164 rx_pwidth=rx/180/1000+0.001;
takeuchi 1:a4ab99c0f7ea 165 px_pwidth=px/180/1000+0.001;
takeuchi 1:a4ab99c0f7ea 166 ax_pwidth=ax/180/1000+0.001;
takeuchi 1:a4ab99c0f7ea 167
takeuchi 1:a4ab99c0f7ea 168 servo1.pulsewidth(rx_pwidth);
takeuchi 1:a4ab99c0f7ea 169 servo2.pulsewidth(px_pwidth);
takeuchi 1:a4ab99c0f7ea 170 servo3.pulsewidth(ax_pwidth);
takeuchi 1:a4ab99c0f7ea 171
takeuchi 1:a4ab99c0f7ea 172 lcd.locate(0,1);
takeuchi 1:a4ab99c0f7ea 173 //lcd.printf("---------+---------");
takeuchi 1:a4ab99c0f7ea 174 //lcd.printf("R:%4.2fms,P:%4.2fms",rx_pwidth*1000,px_pwidth*1000);
takeuchi 1:a4ab99c0f7ea 175 lcd.locate(rx_lc_old,1);
takeuchi 1:a4ab99c0f7ea 176 lcd.printf(" ");
takeuchi 1:a4ab99c0f7ea 177 lcd.locate(rx_lc,1);
takeuchi 1:a4ab99c0f7ea 178 lcd.printf("*");
takeuchi 1:a4ab99c0f7ea 179 rx_lc_old=rx_lc;
takeuchi 1:a4ab99c0f7ea 180 lcd.locate(px_lc_old,2);
takeuchi 1:a4ab99c0f7ea 181 lcd.printf(" ");
takeuchi 1:a4ab99c0f7ea 182 lcd.locate(px_lc,2);
takeuchi 1:a4ab99c0f7ea 183 lcd.printf("*");
takeuchi 1:a4ab99c0f7ea 184 px_lc_old=px_lc;
takeuchi 1:a4ab99c0f7ea 185 lcd.locate(ax_lc_old,3);
takeuchi 1:a4ab99c0f7ea 186 lcd.printf(" ");
takeuchi 1:a4ab99c0f7ea 187 lcd.locate(ax_lc,3);
takeuchi 1:a4ab99c0f7ea 188 lcd.printf("*");
takeuchi 1:a4ab99c0f7ea 189 ax_lc_old=ax_lc;
takeuchi 1:a4ab99c0f7ea 190
takeuchi 1:a4ab99c0f7ea 191 }//while
takeuchi 1:a4ab99c0f7ea 192 }//main
takeuchi 1:a4ab99c0f7ea 193
takeuchi 1:a4ab99c0f7ea 194
takeuchi 1:a4ab99c0f7ea 195