Dave Lu
/
FYDP_Final2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
FYDP_Final2
by 
Mark Vandermeulen
Revision 5:d2e955a94940, committed 2015-03-25
- Comitter:
- tntmarket
- Date:
- Wed Mar 25 09:58:50 2015 +0000
- Parent:
- 4:05484073a641
- Commit message:
- only imu output
Changed in this revision
--- a/bt_shell/machine_interface/ros_machine_interface.h Sun Mar 22 06:34:30 2015 +0000
+++ b/bt_shell/machine_interface/ros_machine_interface.h Wed Mar 25 09:58:50 2015 +0000
@@ -168,7 +168,7 @@
}
fwd_flag = 1;
}
- moveMotors(arg_in[0]-m_offset,arg_in[1]-m_offset,arg_in[2]);
+// moveMotors(arg_in[0]-m_offset,arg_in[1]-m_offset,arg_in[2]);
bt.lock();
bt.printf("L=%d;R=%d;t=%d",arg_in[0],arg_in[1],arg_in[2]); //don't need to respond. Maybe have a confirmation?
bt.unlock();
--- a/bt_shell/shell/motor_fnt.h Sun Mar 22 06:34:30 2015 +0000
+++ b/bt_shell/shell/motor_fnt.h Wed Mar 25 09:58:50 2015 +0000
@@ -45,8 +45,6 @@
bt.printf("moving duration: %sms\r\n", argv[3]);
bt.unlock();
}
-
- //moveMotors(param[0],param[1],param[2]);
}
else
{
--- a/bt_shell/shell/remotecontrol_fnt.h Sun Mar 22 06:34:30 2015 +0000
+++ b/bt_shell/shell/remotecontrol_fnt.h Wed Mar 25 09:58:50 2015 +0000
@@ -1,7 +1,7 @@
void remotecontrol_fnt(int argc, char **argv)
{
int speed = 40;
-
+
bt.lock();
bt.printf("Remote Control Mode\r\n");
bt.printf("Controls:\r\n");
@@ -9,95 +9,90 @@
bt.printf(" r/f for speed +/-\r\n");
bt.printf(" enter key to exit\r\n");
bt.unlock();
-
- if(argc == 2){
- speed = tinysh_atoxi(argv[1]);
+
+ if(argc == 2) {
+ speed = tinysh_atoxi(argv[1]);
if( speed <= 0 || speed > 100)
speed = 100;
- }
-
+ }
+
int Lspeed = 0;
int Rspeed = 0;
char c = 0;
-
- while(c != '\r' && c != '\n')
- {
+
+ while(c != '\r' && c != '\n') {
bt.lock();
-
- if(bt.readable())
- {
- c = bt.getc();
- bt.unlock();
-
- switch ( c ) {
- case 'w':
- Lspeed = speed;
- Rspeed = speed;
- break;
- case 'a':
- Lspeed = -speed;
- Rspeed = speed;
- break;
- case 's':
- Lspeed = -speed;
- Rspeed = -speed;
- break;
- case 'x':
- Lspeed = -speed;
- Rspeed = -speed;
- break;
- case 'd':
- Lspeed = speed;
- Rspeed = -speed;
- break;
- case 'q':
- Lspeed = 0;
- Rspeed = speed;
- break;
- case 'e':
- Lspeed = speed;
- Rspeed = 0;
- break;
- case 'c':
- Lspeed = -speed;
- Rspeed = 0;
- break;
- case 'z':
- Lspeed = 0;
- Rspeed = -speed;
- break;
- case 'r':
- speed += 10;
- if(speed > 100)
- speed = 100;
- bt.lock();
- bt.printf("speed = %d\r\n", speed);
- bt.unlock();
- Lspeed = 0;
- Rspeed = 0;
- break;
- case 'f':
- speed -= 10;
- if(speed < 0)
- speed = 0;
- bt.lock();
- bt.printf("speed = %d\r\n", speed);
- bt.unlock();
- Lspeed = 0;
- Rspeed = 0;
- break;
- default:
- Lspeed = 0;
- Rspeed = 0;
- break;
- }
-
- moveMotors(Lspeed,Rspeed,100);
-
- }else
+
+ if(bt.readable()) {
+ c = bt.getc();
bt.unlock();
-
- Thread::wait(25);
+
+ switch ( c ) {
+ case 'w':
+ Lspeed = speed;
+ Rspeed = speed;
+ break;
+ case 'a':
+ Lspeed = -speed;
+ Rspeed = speed;
+ break;
+ case 's':
+ Lspeed = -speed;
+ Rspeed = -speed;
+ break;
+ case 'x':
+ Lspeed = -speed;
+ Rspeed = -speed;
+ break;
+ case 'd':
+ Lspeed = speed;
+ Rspeed = -speed;
+ break;
+ case 'q':
+ Lspeed = 0;
+ Rspeed = speed;
+ break;
+ case 'e':
+ Lspeed = speed;
+ Rspeed = 0;
+ break;
+ case 'c':
+ Lspeed = -speed;
+ Rspeed = 0;
+ break;
+ case 'z':
+ Lspeed = 0;
+ Rspeed = -speed;
+ break;
+ case 'r':
+ speed += 10;
+ if(speed > 100)
+ speed = 100;
+ bt.lock();
+ bt.printf("speed = %d\r\n", speed);
+ bt.unlock();
+ Lspeed = 0;
+ Rspeed = 0;
+ break;
+ case 'f':
+ speed -= 10;
+ if(speed < 0)
+ speed = 0;
+ bt.lock();
+ bt.printf("speed = %d\r\n", speed);
+ bt.unlock();
+ Lspeed = 0;
+ Rspeed = 0;
+ break;
+ default:
+ Lspeed = 0;
+ Rspeed = 0;
+ break;
+ }
+ } else
+ bt.unlock();
+
+ Thread::wait(25);
}
}
\ No newline at end of file
--- a/main.cpp Sun Mar 22 06:34:30 2015 +0000
+++ b/main.cpp Wed Mar 25 09:58:50 2015 +0000
@@ -1,47 +1,10 @@
-/* This is the current working code for the FYDP Autowalk
--only IMU1 works. I don't know why IMU2 works. I'll fix that later
-*/
-
#include "robot.h"
#include "bt_shell.h"
-RtosTimer *MotorCMDTimer;
-RtosTimer *HalfHourTimer;
-void HalfHourUpdate(void const *args)
-{
- long_time += t.read_us();
- t.reset();
-}
-
-/*
-RtosTimer *OpticalFlowTimer;
-
-void update_opt(void const *args)
-{
- reckon.updateOpticalFlow();
-}
-
-void RF_mesh_thread(void const *args) //this thread handles the mesh network
-{
- while(1){
- Thread::wait(10);
- }
-}
-
-*/
-
-
-void moveMotors(int i, int j, int k)
-{
- //This function does nothing. LEAVE BLANK
- //myservo = 1;
- Thread::yield();
-}
void motor_t(void const *args)
{
-
double angle = 1;
while(1){
//move the servo so it reflects the IMU ROLL
@@ -57,36 +20,8 @@
}
}
-void stopMotors(void const *args)
-{
- *motors.Left = 0;
- *motors.Right = 0;
- send_flag = 0;
-}
-
-void CURRENT_thread(void const *args)
-{
- float current;
- while(1){
- current = current_sensor.get_current();
- bt.lock();
- bt.printf("\n\rCurrent: %f mA",current);
- bt.unlock();
- Thread::wait(50);
- }
-}
-
-void optflow_thread(void const *args)
-{
- while(true) {
- reckon.updateOpticalFlow();
- Thread::wait(3);
- }
-}
-
void IMU2_thread(void const *args)
{
- //This thread does not work
test_dmp2();
start_dmp2(mpu2);
while(1)
@@ -97,21 +32,13 @@
}
void IMU_thread(void const *args)
{
- //For some reason, using 2 IMUs causes one of them to be laggy and
- //the connection fails after a while (buffer overflow??)
- bt.baud(BT_BAUD_RATE); //you have to do this for some reason
- test_dmp(); //test IMU1
- //test_dmp2(); //test IMU2
- bt.baud(BT_BAUD_RATE); //you have to do this for some reason
+ bt.baud(BT_BAUD_RATE);
+ test_dmp();
+ bt.baud(BT_BAUD_RATE);
start_dmp(mpu);
- // start_dmp2(mpu2);
while(1) {
update_dmp();
- //update_dmp2();
- //mpuInterrupt = false; //this resets the interrupt flag
- //mpuInterrupt2 = false;
- //Thread::wait(10);
Thread::yield();
}
}
@@ -124,34 +51,19 @@
while(true) {
bt_shell_run();
Thread::wait(10);
- //Thread::yield();
}
}
int main()
{
initRobot();
-
- //MotorCMDTimer = new RtosTimer(&stopMotors, osTimerOnce, NULL);
- //HalfHourTimer = new RtosTimer(&HalfHourUpdate, osTimerPeriodic, NULL);
-
- //HalfHourTimer->start(1800000); //doesn't work because the timer takes over (high priority)
-
- //OpticalFlowTimer = new RtosTimer(&update_opt, osTimerPeriodic, NULL);
- //OpticalFlowTimer->start(10); //doesn't work because the timer takes over (high priority)
-
- Thread bt_shell_th(bt_shell, NULL, osPriorityNormal, 2048, NULL); //if you get a hard fault, increase memory size of this thread (second last value)
+ Thread bt_shell_th(bt_shell, NULL, osPriorityNormal, 2048, NULL);
Thread IMU_th(IMU_thread,NULL,osPriorityNormal, 2048,NULL);
- Thread IMU2_th(IMU2_thread,NULL,osPriorityNormal, 2048,NULL); //Putting IMU2 in a separate thread does not work
-
-// Thread current_sense(CURRENT_thread,NULL,osPriorityNormal,300,NULL); //this thread for monitoring current is unnecessary. Thread use lotsa rams. Kill it.
-
+ Thread IMU2_th(IMU2_thread,NULL,osPriorityNormal, 2048,NULL);
Thread motor_th(motor_t, NULL, osPriorityNormal, 2048, NULL);
- //Thread RF_mesh_th(RF_mesh_thread,NULL,osPriorityNormal,500,NULL);
-
Thread::wait(osWaitForever);
}
\ No newline at end of file
--- a/robot/robot.h Sun Mar 22 06:34:30 2015 +0000 +++ b/robot/robot.h Wed Mar 25 09:58:50 2015 +0000 @@ -122,6 +122,5 @@ float getTime(); void initRobot(); -void moveMotors(int Lspeed, int Rspeed, int ms); #endif \ No newline at end of file