Wouter Bos / Mbed 2 deprecated Motor_potbesturing_notgradual

motor aansturing moet lineair zijn is het niet

Dependencies:   MODSERIAL Encoder mbed HIDScope

main.cpp@10:b2742f42de44, 2015-10-02 (annotated)

Committer:
Zeekat
Date:
Fri Oct 02 11:31:50 2015 +0000
Revision:
10:b2742f42de44
Parent:
9:f907915f269c
Child:
11:d31b03b05f59
added radians calc and HIDScope

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Zeekat 0:5ea1875b307a 1 #include "mbed.h"
Zeekat 4:e171c9fa5447 2 #include "MODSERIAL.h"
Zeekat 9:f907915f269c 3 #include "encoder.h"
Zeekat 10:b2742f42de44 4 #include "HIDScope.h"
Zeekat 4:e171c9fa5447 5
Zeekat 4:e171c9fa5447 6 Serial pc(USBTX,USBRX);
Zeekat 10:b2742f42de44 7 HIDScope scope(2);
Zeekat 4:e171c9fa5447 8
Zeekat 4:e171c9fa5447 9 Ticker mod;
Zeekat 0:5ea1875b307a 10
Zeekat 0:5ea1875b307a 11 //MOTOR
Zeekat 0:5ea1875b307a 12 // motor 1 en 2 zijn omgedraait
Zeekat 0:5ea1875b307a 13 // pinnen motor 1 -> pinnen motor 2
Zeekat 0:5ea1875b307a 14
Zeekat 0:5ea1875b307a 15 //motor 1 gegevens
Zeekat 0:5ea1875b307a 16 PwmOut motor1_aan(D6); // PWM signaal motor 2 (uit sheets)
Zeekat 0:5ea1875b307a 17 DigitalOut motor1_rich(D7); // digitaal signaal voor richting
Zeekat 0:5ea1875b307a 18 // einde motor 1
Zeekat 0:5ea1875b307a 19
Zeekat 0:5ea1875b307a 20 //motor 2 gegevens
Zeekat 0:5ea1875b307a 21 PwmOut motor2_aan(D5); // PWM signaal motor 1 (uit sheets)
Zeekat 0:5ea1875b307a 22 DigitalOut motor2_rich(D4); // digitaal signaal voor richting
Zeekat 0:5ea1875b307a 23 // einde motor 1
Zeekat 0:5ea1875b307a 24 //EINDE MOTOR
Zeekat 0:5ea1875b307a 25
Zeekat 9:f907915f269c 26 // ENCODER
Zeekat 9:f907915f269c 27 Encoder motor1_enc(D12,D11);
Zeekat 9:f907915f269c 28 Encoder motor2_enc(D10,D9);
Zeekat 4:e171c9fa5447 29
Zeekat 0:5ea1875b307a 30
Zeekat 0:5ea1875b307a 31 //POTMETERS
Zeekat 0:5ea1875b307a 32 AnalogIn potleft(A1);
Zeekat 0:5ea1875b307a 33 AnalogIn potright(A0);
Zeekat 0:5ea1875b307a 34
Zeekat 5:46dae55f0ab0 35 DigitalIn button(PTA4);
Zeekat 5:46dae55f0ab0 36 int button_on = 0;
Zeekat 4:e171c9fa5447 37
Zeekat 9:f907915f269c 38
Zeekat 8:83d522d8f2b8 39 void move_mot1(double left)
Zeekat 0:5ea1875b307a 40 {
Zeekat 10:b2742f42de44 41 if(left < 0.4)
Zeekat 1:8918d924f625 42 {
Zeekat 8:83d522d8f2b8 43 double calc1 = left - 1;
Zeekat 8:83d522d8f2b8 44 double calc2 = abs(calc1);
Zeekat 8:83d522d8f2b8 45 double leftin1 = (calc2-0.6)*2.5 ;
Zeekat 6:3031e8abb052 46 motor1_aan.write(leftin1);
Zeekat 0:5ea1875b307a 47 motor1_rich.write(0);
Zeekat 0:5ea1875b307a 48 }
Zeekat 10:b2742f42de44 49 else if(left > 0.4 && left < 0.6)
Zeekat 0:5ea1875b307a 50 {
Zeekat 0:5ea1875b307a 51 motor1_aan.write(0);
Zeekat 0:5ea1875b307a 52 }
Zeekat 10:b2742f42de44 53 else if(left > 0.6)
Zeekat 0:5ea1875b307a 54 {
Zeekat 8:83d522d8f2b8 55 double leftin2 = (left-0.6)*2.5;
Zeekat 6:3031e8abb052 56 motor1_aan.write(leftin2);
Zeekat 0:5ea1875b307a 57 motor1_rich.write(1);
Zeekat 0:5ea1875b307a 58 }
Zeekat 0:5ea1875b307a 59 }
Zeekat 0:5ea1875b307a 60
Zeekat 8:83d522d8f2b8 61 void move_mot2(double right)
Zeekat 0:5ea1875b307a 62 {
Zeekat 10:b2742f42de44 63 if(right < 0.4)
Zeekat 0:5ea1875b307a 64 {
Zeekat 8:83d522d8f2b8 65 double calc3 = right - 1;
Zeekat 8:83d522d8f2b8 66 double calc4 = abs(calc3);
Zeekat 8:83d522d8f2b8 67 double rightin1 = (calc4-0.6)*2.5 ;
Zeekat 6:3031e8abb052 68 motor2_aan.write(rightin1);
Zeekat 0:5ea1875b307a 69 motor2_rich.write(0);
Zeekat 0:5ea1875b307a 70 }
Zeekat 10:b2742f42de44 71 else if(right > 0.4 && right < 0.6)
Zeekat 0:5ea1875b307a 72 {
Zeekat 0:5ea1875b307a 73 motor2_aan.write(0);
Zeekat 0:5ea1875b307a 74 }
Zeekat 10:b2742f42de44 75 else if(right > 0.6)
Zeekat 0:5ea1875b307a 76 {
Zeekat 8:83d522d8f2b8 77 double rightin2 = (right-0.6)*2.5;
Zeekat 6:3031e8abb052 78 motor2_aan.write(rightin2);
Zeekat 0:5ea1875b307a 79 motor2_rich.write(1);
Zeekat 0:5ea1875b307a 80 }
Zeekat 0:5ea1875b307a 81 }
Zeekat 0:5ea1875b307a 82
Zeekat 4:e171c9fa5447 83 void send()
Zeekat 4:e171c9fa5447 84 {
Zeekat 9:f907915f269c 85 int counts1 = motor1_enc.getPosition();
Zeekat 9:f907915f269c 86 int counts2 = motor2_enc.getPosition();
Zeekat 9:f907915f269c 87 pc.printf("motor 1 counts, %d motor 2 counts %d \n", counts1, counts2);
Zeekat 10:b2742f42de44 88
Zeekat 9:f907915f269c 89 }
Zeekat 10:b2742f42de44 90
Zeekat 10:b2742f42de44 91 double get_radians(double counts)
Zeekat 10:b2742f42de44 92 {
Zeekat 10:b2742f42de44 93 double pi = 3.14159265359;
Zeekat 10:b2742f42de44 94 double radians = (counts/4200)*2*pi;
Zeekat 10:b2742f42de44 95 return radians;
Zeekat 10:b2742f42de44 96 }
Zeekat 10:b2742f42de44 97
Zeekat 4:e171c9fa5447 98
Zeekat 0:5ea1875b307a 99
Zeekat 0:5ea1875b307a 100 int main()
Zeekat 0:5ea1875b307a 101 {
Zeekat 4:e171c9fa5447 102 pc.baud(115200);
Zeekat 4:e171c9fa5447 103 mod.attach(&send,1);
Zeekat 0:5ea1875b307a 104 while(true)
Zeekat 0:5ea1875b307a 105 {
Zeekat 8:83d522d8f2b8 106 double left = potleft.read();
Zeekat 9:f907915f269c 107 double right = potright.read();
Zeekat 4:e171c9fa5447 108 move_mot1(left);
Zeekat 4:e171c9fa5447 109 move_mot2(right);
Zeekat 10:b2742f42de44 110 int counts1 = motor1_enc.getPosition();
Zeekat 10:b2742f42de44 111 int counts2 = motor2_enc.getPosition();
Zeekat 10:b2742f42de44 112 double radians_1 = get_radians(counts1);
Zeekat 10:b2742f42de44 113 double radians_2 = get_radians(counts2);
Zeekat 10:b2742f42de44 114
Zeekat 9:f907915f269c 115 if( button.read() == button_on)
Zeekat 7:eb239942830d 116 {
Zeekat 9:f907915f269c 117 motor1_enc.setPosition(0);
Zeekat 9:f907915f269c 118 motor2_enc.setPosition(0);
Zeekat 7:eb239942830d 119 }
Zeekat 10:b2742f42de44 120 scope.set(0,radians_1);
Zeekat 10:b2742f42de44 121 scope.set(1,radians_2);
Zeekat 10:b2742f42de44 122 scope.send();
Zeekat 9:f907915f269c 123 wait(0.01f); // is noodzakelijk voor functioneren gradual increase, waarom????
Zeekat 4:e171c9fa5447 124
Zeekat 0:5ea1875b307a 125 }
Zeekat 0:5ea1875b307a 126 }