K K / Mbed 2 deprecated Movement_code

code for basic movement of robot

Dependencies:   MODSERIAL QEI mbed

main.cpp@8:e0fc6dd187a2, 2015-10-11 (annotated)

Committer:
Technical_Muffin
Date:
Sun Oct 11 14:48:36 2015 +0000
Revision:
8:e0fc6dd187a2
Parent:
7:46d9f01afba2
Child:
9:80dad8d81c33
untested movement code for two motors

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Technical_Muffin 0:e2de1fe5b34c 1 #include "mbed.h"
Technical_Muffin 0:e2de1fe5b34c 2 #include "QEI.h"
Technical_Muffin 0:e2de1fe5b34c 3 #include "MODSERIAL.h"
Technical_Muffin 0:e2de1fe5b34c 4 //#include <"math.h">
Technical_Muffin 0:e2de1fe5b34c 5
Technical_Muffin 8:e0fc6dd187a2 6 QEI motor1(D13,D12,NC, 624);//encoder for motor 1
Technical_Muffin 8:e0fc6dd187a2 7 QEI motor2(D11,D10,NC, 624);//encoder for motor 2
Technical_Muffin 0:e2de1fe5b34c 8 MODSERIAL pc(USBTX,USBRX);
Technical_Muffin 8:e0fc6dd187a2 9 DigitalOut direction1(D7);//direction input for motor 1
Technical_Muffin 8:e0fc6dd187a2 10 DigitalOut direction2(D4);//direction input for motor 2
Technical_Muffin 8:e0fc6dd187a2 11 PwmOut speed1(D6);//speed input for motor 1
Technical_Muffin 8:e0fc6dd187a2 12 PwmOut speed2(D5);//speed input for motor 2
Technical_Muffin 8:e0fc6dd187a2 13 DigitalIn button1(PTC6);//test button for starting motor 1
Technical_Muffin 8:e0fc6dd187a2 14 DigitalIn button2(PTA4);//test button for starting motor 2
Technical_Muffin 2:27fe9488ba61 15 DigitalOut led1(LED_RED);
Technical_Muffin 2:27fe9488ba61 16 DigitalOut led2(LED_BLUE);
Technical_Muffin 2:27fe9488ba61 17 DigitalOut led3(LED_GREEN);
Technical_Muffin 0:e2de1fe5b34c 18
Technical_Muffin 0:e2de1fe5b34c 19 int main()
Technical_Muffin 3:b913f8ea69e8 20 {
Technical_Muffin 7:46d9f01afba2 21 float cycle = 0.7f;//define the speed of the motor
Technical_Muffin 8:e0fc6dd187a2 22 bool motor1_on = 1;//set the on variable of motor 1
Technical_Muffin 8:e0fc6dd187a2 23 int motor1_dir=0;//set the direction of motor 1
Technical_Muffin 8:e0fc6dd187a2 24 bool motor2_on =1;//set the on variable of motor 2
Technical_Muffin 8:e0fc6dd187a2 25 int motor2_dir = 0;//set the direction of motor 1
Technical_Muffin 0:e2de1fe5b34c 26
Technical_Muffin 3:b913f8ea69e8 27 while(1){
Technical_Muffin 3:b913f8ea69e8 28 led3.write(0);
Technical_Muffin 3:b913f8ea69e8 29 led1.write(1);
Technical_Muffin 3:b913f8ea69e8 30 led2.write(1);
Technical_Muffin 8:e0fc6dd187a2 31 speed1.write(0);//set motor 1 speed to 0
Technical_Muffin 8:e0fc6dd187a2 32 speed2.write(0);//set motor 2 speed to 0
Technical_Muffin 8:e0fc6dd187a2 33 pc.baud(115200);
Technical_Muffin 8:e0fc6dd187a2 34
Technical_Muffin 8:e0fc6dd187a2 35 int diffa1 = button1.read();//read out the button 1 signal and calculate if it is being pressed or released
Technical_Muffin 8:e0fc6dd187a2 36 wait(0.2);//from this we can determine if the rotation direction needs to be reversed.
Technical_Muffin 8:e0fc6dd187a2 37 int diffb1 = button1.read();
Technical_Muffin 8:e0fc6dd187a2 38 int button_toggle1 = diffa1-diffb1;
Technical_Muffin 8:e0fc6dd187a2 39 if(button_toggle1 == 1 && motor1_dir == 1){
Technical_Muffin 7:46d9f01afba2 40 motor1_dir = 0;
Technical_Muffin 7:46d9f01afba2 41 }
Technical_Muffin 8:e0fc6dd187a2 42 else if(button_toggle1 == 1 && motor1_dir == 0){
Technical_Muffin 7:46d9f01afba2 43 motor1_dir = 1;
Technical_Muffin 7:46d9f01afba2 44 }
Technical_Muffin 3:b913f8ea69e8 45
Technical_Muffin 3:b913f8ea69e8 46 while(button1.read() != motor1_on){// turn on motor 1 when the button is being pressed
Technical_Muffin 2:27fe9488ba61 47 led3.write(1);
Technical_Muffin 2:27fe9488ba61 48 led1.write(0);
Technical_Muffin 6:206fff3b20b0 49 speed1.write(cycle);//write the speed to the motor
Technical_Muffin 6:206fff3b20b0 50 speed1.period_us(100);//Set period of PWM to 100 us.
Technical_Muffin 6:206fff3b20b0 51 pc.printf("%f",speed1.read());//klopt nog niet, maar voorlopig zorgt het uitlezen ervoor dat het werkt.
Technical_Muffin 5:a1376ab695f3 52 direction1.write(motor1_dir);//turn motor CCW or CW
Technical_Muffin 0:e2de1fe5b34c 53 //motor CW = 0
Technical_Muffin 0:e2de1fe5b34c 54 //motor CCW = 1
Technical_Muffin 6:206fff3b20b0 55 }
Technical_Muffin 8:e0fc6dd187a2 56
Technical_Muffin 8:e0fc6dd187a2 57 int diffa2 = button2.read();//read out the button 2 signal and calculate if it is being pressed or released
Technical_Muffin 8:e0fc6dd187a2 58 wait(0.2);//from this we can determine if the rotation direction needs to be reversed.
Technical_Muffin 8:e0fc6dd187a2 59 int diffb2 = button2.read();
Technical_Muffin 8:e0fc6dd187a2 60 int button_toggle2 = diffa2-diffb2;
Technical_Muffin 8:e0fc6dd187a2 61 if(button_toggle2 == 1 && motor2_dir == 1){
Technical_Muffin 8:e0fc6dd187a2 62 motor2_dir = 0;
Technical_Muffin 8:e0fc6dd187a2 63 }
Technical_Muffin 8:e0fc6dd187a2 64 else if(button_toggle2 == 1 && motor2_dir == 0){
Technical_Muffin 8:e0fc6dd187a2 65 motor2_dir = 1;
Technical_Muffin 8:e0fc6dd187a2 66 }
Technical_Muffin 8:e0fc6dd187a2 67 while(button2.read() != motor2_on){// turn on motor 2 when the button is being pressed
Technical_Muffin 8:e0fc6dd187a2 68 led3.write(1);
Technical_Muffin 8:e0fc6dd187a2 69 led2.write(0);
Technical_Muffin 8:e0fc6dd187a2 70 speed2.write(cycle);//write the speed to the motor
Technical_Muffin 8:e0fc6dd187a2 71 speed2.period_us(100);//Set period of PWM to 100 us.
Technical_Muffin 8:e0fc6dd187a2 72 pc.printf("%f",speed2.read());//klopt nog niet, maar voorlopig zorgt het uitlezen ervoor dat het werkt.
Technical_Muffin 8:e0fc6dd187a2 73 direction2.write(motor2_dir);//turn motor CCW or CW
Technical_Muffin 8:e0fc6dd187a2 74 //motor CW = 0
Technical_Muffin 8:e0fc6dd187a2 75 //motor CCW = 1
Technical_Muffin 8:e0fc6dd187a2 76 }
Technical_Muffin 3:b913f8ea69e8 77 }
Technical_Muffin 8:e0fc6dd187a2 78 }