EdgeBotix / Mbed 2 deprecated eBot_Firmware_V1

Initial Commit

Dependencies:   mbed HC05 QEI MODSERIAL SWSPI mbed-rtos

main.cpp@6:5ab1735265a9, 2014-10-21 (annotated)

Committer:
harshb
Date:
Tue Oct 21 21:59:15 2014 +0000
Revision:
6:5ab1735265a9
Parent:
5:eb706d3e512c 
Initial Commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Throwbot 1:1da89c13dfa1 1 // FORMAT_CODE_START
Throwbot 0:43331220df0d 2 #include "QEI.h"
Throwbot 0:43331220df0d 3 #include "mbed.h"
Throwbot 0:43331220df0d 4 #include "robot.h"
Throwbot 0:43331220df0d 5 #include "rtos.h"
Throwbot 1:1da89c13dfa1 6 #include "MPU6050.h"
Throwbot 1:1da89c13dfa1 7 #include "I2Cdev.h"
Throwbot 1:1da89c13dfa1 8 #include "tone.h"
Throwbot 1:1da89c13dfa1 9 #include "ultrasonic.h"
Throwbot 2:0f80c8a1c236 10 #include "bt_shell.h"
Throwbot 4:81b0de07841f 11 #include "bt_shell_f.h"
Throwbot 2:0f80c8a1c236 12 int Current_Right_pulse=0;
Throwbot 2:0f80c8a1c236 13 int Current_Left_pulse=0;
Throwbot 2:0f80c8a1c236 14 int Linput=0;
Throwbot 2:0f80c8a1c236 15 int Rinput=0;
Throwbot 2:0f80c8a1c236 16 int Change_Right_pulse=0;
Throwbot 2:0f80c8a1c236 17 int Change_Left_pulse=0;
Throwbot 2:0f80c8a1c236 18 int Previous_Left_pulse=0;
Throwbot 2:0f80c8a1c236 19 int Previos_Right_pulse=0;
Throwbot 2:0f80c8a1c236 20 int time_int = 50;
Throwbot 2:0f80c8a1c236 21 int time_factor=1000/time_int ;
Throwbot 3:d1197b5ea68a 22 int Error_Right=0;
Throwbot 3:d1197b5ea68a 23 int Error_Left=0;
Throwbot 3:d1197b5ea68a 24 float L_Kp=0.1;
Throwbot 3:d1197b5ea68a 25 float R_Kp=0.1;
Throwbot 4:81b0de07841f 26 int Last_Error_Left=0;
Throwbot 4:81b0de07841f 27 int Last_Error_Right=0;
Throwbot 3:d1197b5ea68a 28 int previous_Linput= 0 ;
Throwbot 3:d1197b5ea68a 29 int previous_Rinput= 0 ;
Throwbot 2:0f80c8a1c236 30 RtosTimer *Motor_controller_Timer;
Throwbot 2:0f80c8a1c236 31
Throwbot 2:0f80c8a1c236 32 void Motorcontroller(void const *args)
Throwbot 2:0f80c8a1c236 33 {
Throwbot 2:0f80c8a1c236 34
harshb 6:5ab1735265a9 35 Current_Right_pulse= right.getPulses()/10;
harshb 6:5ab1735265a9 36 Current_Left_pulse=left.getPulses()*(-1)/9.85;
Throwbot 2:0f80c8a1c236 37
Throwbot 2:0f80c8a1c236 38 Change_Right_pulse=Current_Right_pulse- Previos_Right_pulse;
Throwbot 2:0f80c8a1c236 39 Change_Left_pulse=Current_Left_pulse- Previous_Left_pulse;
Throwbot 2:0f80c8a1c236 40
Throwbot 2:0f80c8a1c236 41 Previous_Left_pulse=Current_Left_pulse;
Throwbot 2:0f80c8a1c236 42 Previos_Right_pulse=Current_Right_pulse;
Throwbot 2:0f80c8a1c236 43
Throwbot 3:d1197b5ea68a 44 Error_Right=(Rmotor_speed- (Change_Right_pulse*time_factor) );
Throwbot 3:d1197b5ea68a 45 Error_Left=(Lmotor_speed- (Change_Left_pulse*time_factor));
Throwbot 2:0f80c8a1c236 46 Linput=(Linput+Error_Left*L_Kp);
Throwbot 2:0f80c8a1c236 47 Rinput=(Rinput+Error_Right*R_Kp);
Throwbot 2:0f80c8a1c236 48 if(Linput>100)
Throwbot 2:0f80c8a1c236 49 Linput=100;
Throwbot 2:0f80c8a1c236 50 else if (Linput<-100)
Throwbot 2:0f80c8a1c236 51 Linput= -100;
Throwbot 2:0f80c8a1c236 52 if(Rinput>100)
Throwbot 2:0f80c8a1c236 53 Rinput=100;
Throwbot 2:0f80c8a1c236 54 else if(Rinput<-100)
Throwbot 2:0f80c8a1c236 55 Rinput= -100;
Throwbot 4:81b0de07841f 56 if(Error_Right==0) {
Throwbot 4:81b0de07841f 57 Last_Error_Right++;
Throwbot 3:d1197b5ea68a 58 } else {
Throwbot 4:81b0de07841f 59 Last_Error_Right=0;
Throwbot 4:81b0de07841f 60 }
Throwbot 4:81b0de07841f 61 if(Error_Left==0) {
Throwbot 4:81b0de07841f 62 Last_Error_Left++;
Throwbot 4:81b0de07841f 63 } else {
Throwbot 4:81b0de07841f 64 Last_Error_Left=0;
Throwbot 3:d1197b5ea68a 65 }
Throwbot 4:81b0de07841f 66 if(Last_Error_Right==100) {
Throwbot 4:81b0de07841f 67 Rinput=0;
Throwbot 4:81b0de07841f 68 }
Throwbot 4:81b0de07841f 69 if(Last_Error_Left==100) {
Throwbot 4:81b0de07841f 70 Linput=0;
Throwbot 4:81b0de07841f 71 }
harshb 6:5ab1735265a9 72 *motors.Left = Linput; // HARSH change to -Linput for black and Linput for blue
harshb 6:5ab1735265a9 73 *motors.Right = -Rinput; //// HARSH change to Rinput for black and -Rinput for blue
Throwbot 2:0f80c8a1c236 74 }
Throwbot 0:43331220df0d 75 char c;
Throwbot 1:1da89c13dfa1 76 char buffer[10];
Throwbot 2:0f80c8a1c236 77 void bt_shell_thr(void const *args)
Throwbot 2:0f80c8a1c236 78 {
Throwbot 2:0f80c8a1c236 79 while(true) {
Throwbot 4:81b0de07841f 80 if(Selected_robot=='A') {
Throwbot 4:81b0de07841f 81 bt_shell_run();
Throwbot 4:81b0de07841f 82 } else if(Selected_robot=='F') {
Throwbot 4:81b0de07841f 83 bt_shell_f_run();
Throwbot 4:81b0de07841f 84 }
Throwbot 2:0f80c8a1c236 85 Thread::wait(50);
Throwbot 2:0f80c8a1c236 86 }
Throwbot 2:0f80c8a1c236 87 }
Throwbot 1:1da89c13dfa1 88
Throwbot 1:1da89c13dfa1 89 int main()
Throwbot 1:1da89c13dfa1 90 {
Throwbot 1:1da89c13dfa1 91 initRobot();
Throwbot 1:1da89c13dfa1 92 while(buffer[3] != 'O') {
Throwbot 1:1da89c13dfa1 93 while(buffer[3] != 'E') {
Throwbot 1:1da89c13dfa1 94 if(bt.readable()) {
Throwbot 1:1da89c13dfa1 95 bt.gets(buffer, 5);
Throwbot 1:1da89c13dfa1 96 if(buffer[3] == 'E') {
Throwbot 1:1da89c13dfa1 97 bt.printf(">>1B");
Throwbot 1:1da89c13dfa1 98 } else if(buffer[3] == '?') {
Throwbot 2:0f80c8a1c236 99 bt.printf("eBot#2");
Throwbot 1:1da89c13dfa1 100 }
Throwbot 1:1da89c13dfa1 101 }
Throwbot 1:1da89c13dfa1 102 }
Throwbot 1:1da89c13dfa1 103 if(bt.readable()) {
Throwbot 1:1da89c13dfa1 104 bt.gets(buffer, 5);
Throwbot 1:1da89c13dfa1 105 }
Throwbot 1:1da89c13dfa1 106 }
Throwbot 4:81b0de07841f 107 bt_connected=true;
Throwbot 4:81b0de07841f 108 bt.gets(buffer, 2);
Throwbot 4:81b0de07841f 109 if(buffer[0]=='A') {
Throwbot 4:81b0de07841f 110 Selected_robot='A';
Throwbot 5:eb706d3e512c 111 //imperial_march();
Throwbot 4:81b0de07841f 112
Throwbot 4:81b0de07841f 113 } else if(buffer[0]=='F') {
Throwbot 4:81b0de07841f 114 Selected_robot='F';
Throwbot 4:81b0de07841f 115 Led_on();
Throwbot 4:81b0de07841f 116 }
Throwbot 4:81b0de07841f 117 Thread bt_shell_th(bt_shell_thr, NULL, osPriorityNormal, 2048, NULL);
Throwbot 1:1da89c13dfa1 118 Thread IMU_th(Imu_yaw, NULL, osPriorityNormal, 2048, NULL);
Throwbot 1:1da89c13dfa1 119 Thread Encoder_th(encoder_thread, NULL, osPriorityNormal, 2048, NULL);
Throwbot 2:0f80c8a1c236 120 Motor_controller_Timer = new RtosTimer(&Motorcontroller, osTimerPeriodic, NULL);
Throwbot 3:d1197b5ea68a 121 Motor_controller_Timer->start(time_int);
Throwbot 1:1da89c13dfa1 122 while(1) {
Throwbot 4:81b0de07841f 123 if(Selected_robot=='A') {
Throwbot 4:81b0de07841f 124 if(ldrread2()>0.6) {
Throwbot 4:81b0de07841f 125 Led_on();
Throwbot 4:81b0de07841f 126 } else {
Throwbot 4:81b0de07841f 127 Led_off();
Throwbot 4:81b0de07841f 128 }
Throwbot 1:1da89c13dfa1 129 }
Throwbot 2:0f80c8a1c236 130 ultrasonic_run();
Throwbot 2:0f80c8a1c236 131 Thread::yield();
Throwbot 0:43331220df0d 132 }
Throwbot 2:0f80c8a1c236 133 }