K K / Mbed 2 deprecated Movement_code

code for basic movement of robot

Dependencies:   MODSERIAL QEI mbed

main.cpp@12:2d3d7a9ca496, 2015-10-16 (annotated)

Committer:
Technical_Muffin
Date:
Fri Oct 16 09:51:14 2015 +0000
Revision:
12:2d3d7a9ca496
Parent:
11:be7660614c5c
Child:
13:6cea4a9fcf7a
working improved motor code in which speed functions much better, and also removed some magic numbers.; Directional change seems buggy and needs some polishing

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 12:2d3d7a9ca496 21 float cycle = 0.3;//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 12:2d3d7a9ca496 26 int n1=1;//numeric condtions to determine if the speed needs to be increased
Technical_Muffin 11:be7660614c5c 27 int n2=1;
Technical_Muffin 12:2d3d7a9ca496 28 bool CW =1;
Technical_Muffin 12:2d3d7a9ca496 29 bool CCW = 0;
Technical_Muffin 0:e2de1fe5b34c 30
Technical_Muffin 3:b913f8ea69e8 31 while(1){
Technical_Muffin 3:b913f8ea69e8 32 led3.write(0);
Technical_Muffin 3:b913f8ea69e8 33 led1.write(1);
Technical_Muffin 3:b913f8ea69e8 34 led2.write(1);
Technical_Muffin 8:e0fc6dd187a2 35 pc.baud(115200);
Technical_Muffin 8:e0fc6dd187a2 36
Technical_Muffin 8:e0fc6dd187a2 37 int diffa1 = button1.read();//read out the button 1 signal and calculate if it is being pressed or released
Technical_Muffin 8:e0fc6dd187a2 38 wait(0.2);//from this we can determine if the rotation direction needs to be reversed.
Technical_Muffin 8:e0fc6dd187a2 39 int diffb1 = button1.read();
Technical_Muffin 8:e0fc6dd187a2 40 int button_toggle1 = diffa1-diffb1;
Technical_Muffin 12:2d3d7a9ca496 41 if(button_toggle1 == 1 && motor1_dir == CW){
Technical_Muffin 12:2d3d7a9ca496 42 motor1_dir = CCW;
Technical_Muffin 7:46d9f01afba2 43 }
Technical_Muffin 12:2d3d7a9ca496 44 else if(button_toggle1 == 1 && motor1_dir == CCW){
Technical_Muffin 12:2d3d7a9ca496 45 motor1_dir = CW;
Technical_Muffin 7:46d9f01afba2 46 }
Technical_Muffin 11:be7660614c5c 47
Technical_Muffin 12:2d3d7a9ca496 48 if(button1.read() !=motor1_on){//check if button 1 is pressed
Technical_Muffin 11:be7660614c5c 49 led3.write(1);
Technical_Muffin 11:be7660614c5c 50 led1.write(0);
Technical_Muffin 12:2d3d7a9ca496 51 while(n1 == 1){
Technical_Muffin 12:2d3d7a9ca496 52 speed1.write(cycle);//write speed only on first run through the loop
Technical_Muffin 12:2d3d7a9ca496 53 direction1.write(motor1_dir);//turn motor CCW or CW
Technical_Muffin 12:2d3d7a9ca496 54
Technical_Muffin 11:be7660614c5c 55 n1=0;
Technical_Muffin 11:be7660614c5c 56 }
Technical_Muffin 11:be7660614c5c 57 }
Technical_Muffin 12:2d3d7a9ca496 58 else if (button1.read() == motor1_on){//when button is released
Technical_Muffin 12:2d3d7a9ca496 59 while(n1==0){//check if the first run was done
Technical_Muffin 12:2d3d7a9ca496 60 speed1.write(0);//if so set speed to 0 and reset the run counter
Technical_Muffin 11:be7660614c5c 61 n1=1;
Technical_Muffin 11:be7660614c5c 62 }
Technical_Muffin 11:be7660614c5c 63 }
Technical_Muffin 8:e0fc6dd187a2 64
Technical_Muffin 8:e0fc6dd187a2 65 int diffa2 = button2.read();//read out the button 2 signal and calculate if it is being pressed or released
Technical_Muffin 8:e0fc6dd187a2 66 wait(0.2);//from this we can determine if the rotation direction needs to be reversed.
Technical_Muffin 8:e0fc6dd187a2 67 int diffb2 = button2.read();
Technical_Muffin 8:e0fc6dd187a2 68 int button_toggle2 = diffa2-diffb2;
Technical_Muffin 12:2d3d7a9ca496 69 if(button_toggle2 == 1 && motor2_dir == CW){
Technical_Muffin 12:2d3d7a9ca496 70 motor2_dir = CCW;
Technical_Muffin 8:e0fc6dd187a2 71 }
Technical_Muffin 12:2d3d7a9ca496 72 else if(button_toggle2 == 1 && motor2_dir == CCW){
Technical_Muffin 12:2d3d7a9ca496 73 motor2_dir = CW;
Technical_Muffin 8:e0fc6dd187a2 74 }
Technical_Muffin 11:be7660614c5c 75
Technical_Muffin 8:e0fc6dd187a2 76 //motor CW = 0
Technical_Muffin 8:e0fc6dd187a2 77 //motor CCW = 1
Technical_Muffin 12:2d3d7a9ca496 78 if(button2.read()!=motor2_on){//check if button 2 is pressed
Technical_Muffin 11:be7660614c5c 79 led3.write(1);
Technical_Muffin 11:be7660614c5c 80 led2.write(0);
Technical_Muffin 12:2d3d7a9ca496 81 while(n2 == 1){
Technical_Muffin 12:2d3d7a9ca496 82 speed2.write(cycle);//write speed only on first run through the loop
Technical_Muffin 12:2d3d7a9ca496 83 direction2.write(1);//turn motor CCW or CW
Technical_Muffin 12:2d3d7a9ca496 84
Technical_Muffin 11:be7660614c5c 85 n2=0;
Technical_Muffin 11:be7660614c5c 86 }
Technical_Muffin 11:be7660614c5c 87 }
Technical_Muffin 12:2d3d7a9ca496 88 else if (button2.read() == motor2_on){//when button is released
Technical_Muffin 12:2d3d7a9ca496 89 while(n2==0){//check if the first run was done
Technical_Muffin 12:2d3d7a9ca496 90 speed2.write(0);//if so set speed to 0 and reset the run counter
Technical_Muffin 11:be7660614c5c 91 n2=1;
Technical_Muffin 11:be7660614c5c 92 }
Technical_Muffin 12:2d3d7a9ca496 93 }
Technical_Muffin 3:b913f8ea69e8 94 }
Technical_Muffin 8:e0fc6dd187a2 95 }