EdgeBotix / Mbed 2 deprecated eBot_Firmware_V1

Initial Commit

Dependencies:   mbed HC05 QEI MODSERIAL SWSPI mbed-rtos

main.cpp@2:0f80c8a1c236, 2014-10-12 (annotated)

Committer:
Throwbot
Date:
Sun Oct 12 13:33:19 2014 +0000
Revision:
2:0f80c8a1c236
Parent:
1:1da89c13dfa1
Child:
3:d1197b5ea68a
Testing Ultrasonic

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