Pipe Inpection Robot / Mbed OS GPG_motor_latest

complete motor

Dependencies:   BufferedSerial motor_sn7544

main.cpp@5:e6f9768e1438, 2019-06-27 (annotated)

Committer:
kangmingyo
Date:
Thu Jun 27 04:05:20 2019 +0000
Revision:
5:e6f9768e1438
Parent:
4:a72f75611198
Child:
7:13dd93a0efe8
v2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kangmingyo 0:dde6e4d8c301 1 #include "mbed.h"
kangmingyo 1:1d18a2e8ce45 2 #include "motor.h"
kangmingyo 0:dde6e4d8c301 3
kangmingyo 1:1d18a2e8ce45 4 MotorCtl motor1(D3,D12,D4,D5);
kangmingyo 1:1d18a2e8ce45 5 InterruptIn tachoINT1(D4);
kangmingyo 1:1d18a2e8ce45 6 InterruptIn tachoINT2(D5);
kangmingyo 0:dde6e4d8c301 7 Serial pc(USBTX,USBRX,115200);
kangmingyo 2:a04440f0d001 8 char rx_buffer[10];
kangmingyo 2:a04440f0d001 9 int index=0;
kangmingyo 2:a04440f0d001 10 volatile int flag;
kangmingyo 2:a04440f0d001 11 void update_current(void)
kangmingyo 2:a04440f0d001 12 {
kangmingyo 1:1d18a2e8ce45 13 motor1.UpdateCurrentPosition();
kangmingyo 2:a04440f0d001 14 // pc.printf("Update Position\r\n");
kangmingyo 2:a04440f0d001 15 }
kangmingyo 5:e6f9768e1438 16 void rx_cb(void)
kangmingyo 5:e6f9768e1438 17 {
kangmingyo 2:a04440f0d001 18 char ch;
kangmingyo 2:a04440f0d001 19 ch = pc.getc();
kangmingyo 2:a04440f0d001 20 pc.putc(ch);
kangmingyo 2:a04440f0d001 21 rx_buffer[index++]=ch;
kangmingyo 5:e6f9768e1438 22 if(ch==0x0D) {
kangmingyo 2:a04440f0d001 23 pc.putc(0x0A);
kangmingyo 2:a04440f0d001 24 rx_buffer[--index]='\0';
kangmingyo 2:a04440f0d001 25 index=0;
kangmingyo 2:a04440f0d001 26 flag=1;
kangmingyo 2:a04440f0d001 27 }
kangmingyo 1:1d18a2e8ce45 28 }
kangmingyo 1:1d18a2e8ce45 29
kangmingyo 3:0c6258635590 30 void set_speed(void)
kangmingyo 2:a04440f0d001 31 {
kangmingyo 2:a04440f0d001 32 int speed;
kangmingyo 2:a04440f0d001 33 speed = atoi((const char*)rx_buffer);
kangmingyo 5:e6f9768e1438 34 if(speed>78) {
kangmingyo 5:e6f9768e1438 35 speed=78;
kangmingyo 5:e6f9768e1438 36 }
kangmingyo 5:e6f9768e1438 37 if(speed<-78) {
kangmingyo 5:e6f9768e1438 38 speed=-78;
kangmingyo 5:e6f9768e1438 39 }
kangmingyo 2:a04440f0d001 40 motor1.setTarget(speed);
kangmingyo 5:e6f9768e1438 41 // motor1.setTarget(60);
kangmingyo 2:a04440f0d001 42 pc.printf(" Set speed = %d\r\n", speed);
kangmingyo 5:e6f9768e1438 43
kangmingyo 2:a04440f0d001 44 }
kangmingyo 0:dde6e4d8c301 45
kangmingyo 2:a04440f0d001 46 int main()
kangmingyo 2:a04440f0d001 47 {
kangmingyo 1:1d18a2e8ce45 48 tachoINT1.fall(&update_current);
kangmingyo 1:1d18a2e8ce45 49 tachoINT1.rise(&update_current);
kangmingyo 1:1d18a2e8ce45 50 tachoINT2.fall(&update_current);
kangmingyo 1:1d18a2e8ce45 51 tachoINT2.rise(&update_current);
kangmingyo 2:a04440f0d001 52 pc.attach(callback(rx_cb));
kangmingyo 2:a04440f0d001 53 int rpm;
kangmingyo 4:a72f75611198 54 float reladistance; //m단위
kangmingyo 4:a72f75611198 55 float cumdistance;
kangmingyo 2:a04440f0d001 56
kangmingyo 2:a04440f0d001 57 while(1) {
kangmingyo 2:a04440f0d001 58 flag=0;
kangmingyo 5:e6f9768e1438 59
kangmingyo 2:a04440f0d001 60 pc.printf("Enter the value for speed [-78,78]\r\n");
kangmingyo 2:a04440f0d001 61 while(flag!=1) {
kangmingyo 4:a72f75611198 62 rpm = motor1.getRPM();
kangmingyo 4:a72f75611198 63 cumdistance = motor1.CalculateCumDis(); // 누적거리
kangmingyo 4:a72f75611198 64 reladistance = motor1.CalculateRelaDis(); // 상대거리
kangmingyo 5:e6f9768e1438 65
kangmingyo 5:e6f9768e1438 66
kangmingyo 4:a72f75611198 67 printf("Rpm: %f\r\n",distance);
kangmingyo 4:a72f75611198 68 printf("cumdistance: %f\r\n",cumdistance);
kangmingyo 4:a72f75611198 69 printf("reladistance: %f\r\n",reladistance);
kangmingyo 2:a04440f0d001 70 wait(1);
kangmingyo 1:1d18a2e8ce45 71 }
kangmingyo 3:0c6258635590 72 // motor1.setTarget(60);
kangmingyo 2:a04440f0d001 73 set_speed();
kangmingyo 2:a04440f0d001 74 }
kangmingyo 0:dde6e4d8c301 75
kangmingyo 2:a04440f0d001 76 }