![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
test0417
main.cpp
- Committer:
- Tanakacool
- Date:
- 2019-03-27
- Revision:
- 7:46bb04ae559c
- Parent:
- 5:221ce6ba655c
- Child:
- 8:31d4dd20cfc0
File content as of revision 7:46bb04ae559c:
#include "mbed.h" #define LEFT 0 #define RIGHT 1 #define WHEEL_SEPARATION 4 //DigitalOut led_red(LED_RED); DigitalOut led1(LED1); DigitalOut LIFT_UP_PIN(D4); DigitalOut LIFT_DOWN_PIN(D3); //InterruptIn sw2(B1); //I2C i2c( PTB1, PTB0); I2C i2c( D14, D15); Serial serialPort(USBTX, USBRX); //Serial serialPort( PTE0, PTE1); char Rmotobuffer[3]; char Lmotobuffer[3]; int Right_ADDR = 0x14<<1; int Left_ADDR = 0x15<<1; int lin_vel1; //左輪 int lin_vel2; //右輪 int VELOCITY_CONSTANT_VAULE= 17000; int LIMIT_X_MAX_VELOCITY = 8293; //max cmd:8293~=0.5m/s int LIMIT_X_MIN_VELOCITY = 1640; //min value /* void sw2_release(void) { led_red = !led_red; serialPort.printf("On-board button SW2 was released.\n"); } */ void Driving_Right(int rightOutPut); void Driving_Left(int leftOutPut); void liftup(int stage); void controlMotorSpeed(float goalLinearVel, float goalAngularVel); int main() { //sw2.rise(&sw2_release); serialPort.baud(9600); serialPort.printf("WPI Serial Port Started\n"); i2c.frequency(100000); wait_ms(100); int motorOutPut=0; Driving_Right(motorOutPut); wait_ms(100); Driving_Left(motorOutPut); liftup(0); while (true) { } } void controlMotorSpeed(float goalLinearVel, float goalAngularVel) { float wheel_speed_cmd[2]; wheel_speed_cmd[LEFT] = goalLinearVel - (goalAngularVel * WHEEL_SEPARATION / 2); wheel_speed_cmd[RIGHT] = goalLinearVel + (goalAngularVel * WHEEL_SEPARATION / 2); lin_vel1 = wheel_speed_cmd[LEFT] * VELOCITY_CONSTANT_VAULE; wait_ms(2); if (lin_vel1 > LIMIT_X_MAX_VELOCITY) lin_vel1 = LIMIT_X_MAX_VELOCITY; else if (lin_vel1 < LIMIT_X_MIN_VELOCITY && lin_vel1 > 0 ) lin_vel1 = LIMIT_X_MIN_VELOCITY; else if (lin_vel1 < -LIMIT_X_MAX_VELOCITY) lin_vel1 = -LIMIT_X_MAX_VELOCITY; else if (lin_vel1 > -LIMIT_X_MIN_VELOCITY && lin_vel1 < 0) lin_vel1 = -LIMIT_X_MIN_VELOCITY; lin_vel2 = wheel_speed_cmd[RIGHT] * VELOCITY_CONSTANT_VAULE; wait_ms(2); if (lin_vel2 > LIMIT_X_MAX_VELOCITY) lin_vel2 = LIMIT_X_MAX_VELOCITY; else if (lin_vel2 < LIMIT_X_MIN_VELOCITY && lin_vel2 > 0 ) lin_vel2 = LIMIT_X_MIN_VELOCITY; else if (lin_vel2 < -LIMIT_X_MAX_VELOCITY) lin_vel2 = -LIMIT_X_MAX_VELOCITY; else if (lin_vel2 > -LIMIT_X_MIN_VELOCITY && lin_vel2 < 0) lin_vel2 = -LIMIT_X_MIN_VELOCITY; } void liftup(int stage) { switch (stage) { case 0: //stop LIFT_UP_PIN=0; LIFT_DOWN_PIN=0; break; case 1: //up LIFT_UP_PIN=1; LIFT_DOWN_PIN=0; break; case 2: //down LIFT_UP_PIN=0; LIFT_DOWN_PIN=1; break; } } void Driving_Right(int rightOutPut) { Rmotobuffer[0]= 0x00; Rmotobuffer[1]= rightOutPut>>8; Rmotobuffer[2]= rightOutPut; i2c.start(); i2c.write(Right_ADDR); i2c.write(0); i2c.write(Rmotobuffer[1]); i2c.write(Rmotobuffer[2]); i2c.stop(); } void Driving_Left(int leftOutPut) { Lmotobuffer[0]= 0x00; Lmotobuffer[1]= leftOutPut>>8; Lmotobuffer[2]= leftOutPut; i2c.start(); i2c.write(Left_ADDR); i2c.write(0); i2c.write(Lmotobuffer[1]); i2c.write(Lmotobuffer[2]); i2c.stop(); }