Pipe Inpection Robot / Mbed OS GPG_motor_latest

complete motor

Dependencies:   BufferedSerial motor_sn7544

main.cpp@4:a72f75611198, 2019-06-25 (annotated)

Committer:
kangmingyo
Date:
Tue Jun 25 11:07:15 2019 +0000
Revision:
4:a72f75611198
Parent:
3:0c6258635590
Child:
5:e6f9768e1438
value update

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 2:a04440f0d001 16 void rx_cb(void){
kangmingyo 2:a04440f0d001 17 char ch;
kangmingyo 2:a04440f0d001 18 ch = pc.getc();
kangmingyo 2:a04440f0d001 19 pc.putc(ch);
kangmingyo 2:a04440f0d001 20 rx_buffer[index++]=ch;
kangmingyo 2:a04440f0d001 21 if(ch==0x0D){
kangmingyo 2:a04440f0d001 22 pc.putc(0x0A);
kangmingyo 2:a04440f0d001 23 rx_buffer[--index]='\0';
kangmingyo 2:a04440f0d001 24 index=0;
kangmingyo 2:a04440f0d001 25 flag=1;
kangmingyo 2:a04440f0d001 26 }
kangmingyo 1:1d18a2e8ce45 27 }
kangmingyo 1:1d18a2e8ce45 28
kangmingyo 3:0c6258635590 29 void set_speed(void)
kangmingyo 2:a04440f0d001 30 {
kangmingyo 2:a04440f0d001 31 int speed;
kangmingyo 2:a04440f0d001 32 speed = atoi((const char*)rx_buffer);
kangmingyo 2:a04440f0d001 33 if(speed>78){speed=78;}
kangmingyo 2:a04440f0d001 34 if(speed<-78){speed=-78;}
kangmingyo 2:a04440f0d001 35 motor1.setTarget(speed);
kangmingyo 3:0c6258635590 36 // motor1.setTarget(60);
kangmingyo 2:a04440f0d001 37 pc.printf(" Set speed = %d\r\n", speed);
kangmingyo 3:0c6258635590 38
kangmingyo 2:a04440f0d001 39 }
kangmingyo 0:dde6e4d8c301 40
kangmingyo 2:a04440f0d001 41 int main()
kangmingyo 2:a04440f0d001 42 {
kangmingyo 1:1d18a2e8ce45 43 tachoINT1.fall(&update_current);
kangmingyo 1:1d18a2e8ce45 44 tachoINT1.rise(&update_current);
kangmingyo 1:1d18a2e8ce45 45 tachoINT2.fall(&update_current);
kangmingyo 1:1d18a2e8ce45 46 tachoINT2.rise(&update_current);
kangmingyo 2:a04440f0d001 47 pc.attach(callback(rx_cb));
kangmingyo 2:a04440f0d001 48 int rpm;
kangmingyo 4:a72f75611198 49 float reladistance; //m단위
kangmingyo 4:a72f75611198 50 float cumdistance;
kangmingyo 2:a04440f0d001 51
kangmingyo 2:a04440f0d001 52 while(1) {
kangmingyo 2:a04440f0d001 53 flag=0;
kangmingyo 3:0c6258635590 54
kangmingyo 2:a04440f0d001 55 pc.printf("Enter the value for speed [-78,78]\r\n");
kangmingyo 2:a04440f0d001 56 while(flag!=1) {
kangmingyo 4:a72f75611198 57 rpm = motor1.getRPM();
kangmingyo 4:a72f75611198 58 cumdistance = motor1.CalculateCumDis(); // 누적거리
kangmingyo 4:a72f75611198 59 reladistance = motor1.CalculateRelaDis(); // 상대거리
kangmingyo 4:a72f75611198 60
kangmingyo 4:a72f75611198 61
kangmingyo 4:a72f75611198 62 printf("Rpm: %f\r\n",distance);
kangmingyo 4:a72f75611198 63 printf("cumdistance: %f\r\n",cumdistance);
kangmingyo 4:a72f75611198 64 printf("reladistance: %f\r\n",reladistance);
kangmingyo 2:a04440f0d001 65 wait(1);
kangmingyo 1:1d18a2e8ce45 66 }
kangmingyo 3:0c6258635590 67 // motor1.setTarget(60);
kangmingyo 2:a04440f0d001 68 set_speed();
kangmingyo 2:a04440f0d001 69 }
kangmingyo 0:dde6e4d8c301 70
kangmingyo 2:a04440f0d001 71 }