EdgeBotix / Mbed 2 deprecated eBot_Firmware_V1

Initial Commit

Dependencies:   mbed HC05 QEI MODSERIAL SWSPI mbed-rtos

main.cpp@3:d1197b5ea68a, 2014-10-12 (annotated)

Committer:
Throwbot
Date:
Sun Oct 12 23:27:43 2014 +0000
Revision:
3:d1197b5ea68a
Parent:
2:0f80c8a1c236
Child:
4:81b0de07841f
Ebot demo working

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 2:0f80c8a1c236 11 int Current_Right_pulse=0;
Throwbot 2:0f80c8a1c236 12 int Current_Left_pulse=0;
Throwbot 2:0f80c8a1c236 13 int Linput=0;
Throwbot 2:0f80c8a1c236 14 int Rinput=0;
Throwbot 2:0f80c8a1c236 15 int Change_Right_pulse=0;
Throwbot 2:0f80c8a1c236 16 int Change_Left_pulse=0;
Throwbot 2:0f80c8a1c236 17 int Previous_Left_pulse=0;
Throwbot 2:0f80c8a1c236 18 int Previos_Right_pulse=0;
Throwbot 2:0f80c8a1c236 19 int time_int = 50;
Throwbot 2:0f80c8a1c236 20 int time_factor=1000/time_int ;
Throwbot 3:d1197b5ea68a 21 int Error_Right=0;
Throwbot 3:d1197b5ea68a 22 int Error_Left=0;
Throwbot 3:d1197b5ea68a 23 float L_Kp=0.1;
Throwbot 3:d1197b5ea68a 24 float R_Kp=0.1;
Throwbot 3:d1197b5ea68a 25 int previous_Linput= 0 ;
Throwbot 3:d1197b5ea68a 26 int previous_Rinput= 0 ;
Throwbot 2:0f80c8a1c236 27 RtosTimer *Motor_controller_Timer;
Throwbot 2:0f80c8a1c236 28
Throwbot 2:0f80c8a1c236 29 void Motorcontroller(void const *args)
Throwbot 2:0f80c8a1c236 30 {
Throwbot 2:0f80c8a1c236 31
Throwbot 3:d1197b5ea68a 32 Current_Right_pulse= right.getPulses()/5;
Throwbot 3:d1197b5ea68a 33 Current_Left_pulse=left.getPulses()*(-1)/5;
Throwbot 2:0f80c8a1c236 34
Throwbot 2:0f80c8a1c236 35 Change_Right_pulse=Current_Right_pulse- Previos_Right_pulse;
Throwbot 2:0f80c8a1c236 36 Change_Left_pulse=Current_Left_pulse- Previous_Left_pulse;
Throwbot 2:0f80c8a1c236 37
Throwbot 2:0f80c8a1c236 38 Previous_Left_pulse=Current_Left_pulse;
Throwbot 2:0f80c8a1c236 39 Previos_Right_pulse=Current_Right_pulse;
Throwbot 2:0f80c8a1c236 40
Throwbot 3:d1197b5ea68a 41 Error_Right=(Rmotor_speed- (Change_Right_pulse*time_factor) );
Throwbot 3:d1197b5ea68a 42 Error_Left=(Lmotor_speed- (Change_Left_pulse*time_factor));
Throwbot 2:0f80c8a1c236 43 Linput=(Linput+Error_Left*L_Kp);
Throwbot 2:0f80c8a1c236 44 Rinput=(Rinput+Error_Right*R_Kp);
Throwbot 2:0f80c8a1c236 45 if(Linput>100)
Throwbot 2:0f80c8a1c236 46 Linput=100;
Throwbot 2:0f80c8a1c236 47 else if (Linput<-100)
Throwbot 2:0f80c8a1c236 48 Linput= -100;
Throwbot 3:d1197b5ea68a 49 else if (-11<Linput && Linput<11)
Throwbot 2:0f80c8a1c236 50 Linput= 0;
Throwbot 2:0f80c8a1c236 51 if(Rinput>100)
Throwbot 2:0f80c8a1c236 52 Rinput=100;
Throwbot 2:0f80c8a1c236 53 else if(Rinput<-100)
Throwbot 2:0f80c8a1c236 54 Rinput= -100;
Throwbot 3:d1197b5ea68a 55 else if (-11<Rinput && Rinput<11)
Throwbot 2:0f80c8a1c236 56 Rinput= 0;
Throwbot 3:d1197b5ea68a 57 if (Linput== previous_Linput && Rinput ==previous_Rinput) {
Throwbot 3:d1197b5ea68a 58 } else {
Throwbot 3:d1197b5ea68a 59 previous_Linput= Linput ;
Throwbot 3:d1197b5ea68a 60 previous_Rinput= Rinput ;
Throwbot 3:d1197b5ea68a 61 motor_control(-Rinput,-Linput);
Throwbot 3:d1197b5ea68a 62 }
Throwbot 3:d1197b5ea68a 63 /*
Throwbot 3:d1197b5ea68a 64 bt.lock();
Throwbot 3:d1197b5ea68a 65 bt.printf(">>D;%d;%d;%d;%d;%d;%d;%d;%d;\r\n",
Throwbot 3:d1197b5ea68a 66 Rmotor_speed,Lmotor_speed,Error_Right ,Error_Left,\
Throwbot 3:d1197b5ea68a 67 Rinput,Linput,Change_Right_pulse,Change_Left_pulse);
Throwbot 3:d1197b5ea68a 68 bt.unlock();
Throwbot 3:d1197b5ea68a 69 */
Throwbot 2:0f80c8a1c236 70 }
Throwbot 2:0f80c8a1c236 71
Throwbot 2:0f80c8a1c236 72
Throwbot 0:43331220df0d 73 char c;
Throwbot 1:1da89c13dfa1 74 char buffer[10];
Throwbot 2:0f80c8a1c236 75 void bt_shell_thr(void const *args)
Throwbot 2:0f80c8a1c236 76 {
Throwbot 2:0f80c8a1c236 77 while(true) {
Throwbot 2:0f80c8a1c236 78 bt_shell_run();
Throwbot 2:0f80c8a1c236 79 Thread::wait(50);
Throwbot 2:0f80c8a1c236 80 }
Throwbot 2:0f80c8a1c236 81 }
Throwbot 1:1da89c13dfa1 82
Throwbot 1:1da89c13dfa1 83 int main()
Throwbot 1:1da89c13dfa1 84 {
Throwbot 1:1da89c13dfa1 85 initRobot();
Throwbot 1:1da89c13dfa1 86 while(buffer[3] != 'O') {
Throwbot 1:1da89c13dfa1 87 while(buffer[3] != 'E') {
Throwbot 1:1da89c13dfa1 88 if(bt.readable()) {
Throwbot 1:1da89c13dfa1 89 bt.gets(buffer, 5);
Throwbot 1:1da89c13dfa1 90 if(buffer[3] == 'E') {
Throwbot 1:1da89c13dfa1 91 bt.printf(">>1B");
Throwbot 1:1da89c13dfa1 92 } else if(buffer[3] == '?') {
Throwbot 2:0f80c8a1c236 93 bt.printf("eBot#2");
Throwbot 1:1da89c13dfa1 94 }
Throwbot 1:1da89c13dfa1 95 }
Throwbot 1:1da89c13dfa1 96 }
Throwbot 1:1da89c13dfa1 97 if(bt.readable()) {
Throwbot 1:1da89c13dfa1 98 bt.gets(buffer, 5);
Throwbot 1:1da89c13dfa1 99 }
Throwbot 1:1da89c13dfa1 100 }
Throwbot 1:1da89c13dfa1 101 //imperial_march();
Throwbot 1:1da89c13dfa1 102 Thread IMU_th(Imu_yaw, NULL, osPriorityNormal, 2048, NULL);
Throwbot 1:1da89c13dfa1 103 Thread Encoder_th(encoder_thread, NULL, osPriorityNormal, 2048, NULL);
Throwbot 2:0f80c8a1c236 104 Thread bt_shell_th(bt_shell_thr, NULL, osPriorityNormal, 2048, NULL);
Throwbot 2:0f80c8a1c236 105 Motor_controller_Timer = new RtosTimer(&Motorcontroller, osTimerPeriodic, NULL);
Throwbot 3:d1197b5ea68a 106 Motor_controller_Timer->start(time_int);
Throwbot 1:1da89c13dfa1 107 while(1) {
Throwbot 1:1da89c13dfa1 108 if(ldrread1()>0.6) {
Throwbot 1:1da89c13dfa1 109 }
Throwbot 1:1da89c13dfa1 110 if(ldrread2()>0.6) {
Throwbot 1:1da89c13dfa1 111 Led_on();
Throwbot 1:1da89c13dfa1 112 } else {
Throwbot 1:1da89c13dfa1 113 Led_off();
Throwbot 1:1da89c13dfa1 114 }
Throwbot 2:0f80c8a1c236 115 ultrasonic_run();
Throwbot 2:0f80c8a1c236 116 Thread::yield();
Throwbot 0:43331220df0d 117 }
Throwbot 2:0f80c8a1c236 118 }